From e2dfec50a9ac78ba4d8ae3ad534d2d25a10bbee3 Mon Sep 17 00:00:00 2001 From: unghee Date: Thu, 21 Apr 2022 21:00:31 -0400 Subject: [PATCH 01/12] added clipiing, max_allowable_torque for logging --- exoboot.py | 5 +++++ 1 file changed, 5 insertions(+) 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: From 49f56077f425650dd9f65d58564b96264df64b71 Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Fri, 22 Apr 2022 08:52:58 +0100 Subject: [PATCH 02/12] update --- calibrate.py | 10 ++-- config_util.py | 8 +-- constants.py | 32 +++++------ custom_configs/default_config.py | 97 ++++++++++++++++---------------- exoboot.py | 2 +- 5 files changed, 74 insertions(+), 75 deletions(-) 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..5817243 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.075 #0.2 + PEAK_FRACTION: float = 0.33535#0.53 + FALL_FRACTION: float = 0.44478#0.60 + PEAK_TORQUE: float = 18.96235#5 SPLINE_BIAS: float = 3 # Nm # Impedance diff --git a/constants.py b/constants.py index 6605e95..9f65d4c 100644 --- a/constants.py +++ b/constants.py @@ -9,8 +9,8 @@ class ExobootModel(Enum): EB51 = 1 -ExoType: Type[ExobootModel] = ExobootModel.EB51 -#ExoType = Type[ExobootModel] = ExobootModel.EB45 +#ExoType: Type[ExobootModel] = ExobootModel.EB51 +ExoType: Type[ExobootModel] = ExobootModel.EB45 DEFAULT_BAUD_RATE = 230400 TARGET_FREQ = 200 @@ -41,7 +41,7 @@ class ExobootModel(Enum): -11]) # Nm/Nm LEFT_ANKLE_ANGLE_OFFSET = 201 #-92 # 7, - RIGHT_ANKLE_ANGLE_OFFSET = -150 #-200# deg + RIGHT_ANKLE_ANGLE_OFFSET = -198# deg elif (ExoType == ExobootModel.EB51): @@ -50,27 +50,25 @@ class ExobootModel(Enum): MIN_ANKLE_ANGLE = -95 #EB-51 - LEFT_ANKLE_TO_MOTOR = np.array([ - -1.39600219e-06, -1.19669863e-04, 5.18587784e-02, 3.48021800e+00, - -5.71389166e+02, -2.84883844e+04 - ]) + #LEFT_ANKLE_TO_MOTOR = np.array([-2.27233618e-06, 3.55519545e-05, 4.73297332e-02, 3.18534948e+00,-5.63299330e+02, -2.85391796e+04]) + LEFT_ANKLE_TO_MOTOR = np.array([-2.41577803e-06, 6.03503507e-05, 4.69647778e-02, 3.14213596e+00,-5.63452288e+02, 2.04918050e+04]) #LEFT_ANKLE_TO_MOTOR = np.array([-2.32052284e-06, 6.29683551e-05, 4.45431399e-02, 3.09556691e+00,-5.49941515e+02, -1.11211736e+04]) #used for the test trials - RIGHT_ANKLE_TO_MOTOR = np.array([ - 5.05377935e-06, -2.83727629e-04, -7.31477078e-02, -1.56178494e+00, - 6.34368537e+02, 9.35489733e+03 - ]) + RIGHT_ANKLE_TO_MOTOR = np.array([5.05377935e-06, -2.83727629e-04, -7.31477078e-02, -1.56178494e+00, + 6.34368537e+02, 9.35489733e+03]) #RIGHT_ANKLE_TO_MOTOR = np.array([ 5.37977952e-06, -2.76399833e-04, -7.58092528e-02, -1.65003833e+00,6.38860770e+02, 9.21647151e+03]) #used for the test trials #EB-51 + ANKLE_PTS_LEFT = np.array([ - -67, -60, -47, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 80, 90, 100, - 112 + -67, -60, -50, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 70, 80, 86 ]) TR_PTS_LEFT = 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 + 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 ]) + #ANKLE_PTS_LEFT = np.array([-67, -60, -47, -40, -30, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 80, 90,100, 112]) + #TR_PTS_LEFT = 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]) #ANKLE_PTS_LEFT = np.array([-67, -60, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90, 100]) #used for the test trials #TR_PTS_LEFT = 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]) #used for the test trials @@ -91,8 +89,8 @@ class ExobootModel(Enum): # RIGHT_ANKLE_TO_TR = np.array([ 4.83188447e-07, -3.83712114e-05, -3.61934700e-03, 4.54812251e-01, # -2.89416189e+01]) ## NEED TO CHANGE FOR RIGHT ANKLE - #EB-51 - LEFT_ANKLE_ANGLE_OFFSET = -67 # deg + #EB-67#-51 + LEFT_ANKLE_ANGLE_OFFSET = -67#-95#-67 # deg RIGHT_ANKLE_ANGLE_OFFSET = 87.1 #100 # deg # Add to these lists if dev_ids change, or new exos or actpacks are purchased! diff --git a/custom_configs/default_config.py b/custom_configs/default_config.py index 2aa4178..1a78e60 100644 --- a/custom_configs/default_config.py +++ b/custom_configs/default_config.py @@ -1,48 +1,49 @@ - -import config_util -config = config_util.ConfigurableConstants() -# config.HS_GYRO_DELAY = 0.05 # For example -config.MAX_ALLOWABLE_CURRENT = 24000 # mA -config.REEL_IN_MV = 900 -config.REEL_IN_TIMEOUT = 0.075 # 0.2 -config.SWING_SLACK = 3500 #5000 -config.TOE_OFF_FRACTION = 0.65 -config.DO_INCLUDE_GEN_VARS = True - -''' Here are the variables that are updatable in config, and their defaults: - - TARGET_FREQ: float = 200 # Hz - ACTPACK_FREQ: float = 200 # Hz - DO_DEPHY_LOG: bool = True - DEPHY_LOG_LEVEL: int = 4 - TASK: Type[Task] = Task.WALKING - STANCE_CONTROL_STYLE: Type[StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE - MAX_ALLOWABLE_CURRENT = 20000 # mA - - # Gait State details - HS_GYRO_THRESHOLD: float = 100 - HS_GYRO_FILTER_N: int = 2 - HS_GYRO_FILTER_WN: float = 3 - HS_GYRO_DELAY: float = 0.05 - SWING_SLACK: int = 10000 - TOE_OFF_FRACTION: float = 0.62 - REEL_IN_TIMEOUT: float = 0.2 - - # 4 point Spline - RISE_FRACTION: float = 0.2 - PEAK_FRACTION: float = 0.53 - FALL_FRACTION: float = 0.63 - PEAK_TORQUE: float = 5 - SPLINE_BIAS: float = 3 # Nm - - # Impedance - K_VAL: int = 500 - B_VAL: int = 0 - SET_POINT: float = 0 # Deg - - READ_ONLY = False # Does not require Lipos - DO_READ_FSRS = False - - PRINT_HS = True # Print heel strikes - SLIP_DETECT_ACTIVE = False -''' + +import config_util +config = config_util.ConfigurableConstants() +# config.HS_GYRO_DELAY = 0.05 # For example +config.MAX_ALLOWABLE_CURRENT = 25000 # mA +config.REEL_IN_MV = 900 +config.REEL_IN_TIMEOUT = 0.075 # 0.2 +config.SWING_SLACK = 3500 #5000 +config.TOE_OFF_FRACTION = 0.65 +config.DO_INCLUDE_GEN_VARS = True + + +''' Here are the variables that are updatable in config, and their defaults: + + TARGET_FREQ: float = 200 # Hz + ACTPACK_FREQ: float = 200 # Hz + DO_DEPHY_LOG: bool = True + DEPHY_LOG_LEVEL: int = 4 + TASK: Type[Task] = Task.WALKING + STANCE_CONTROL_STYLE: Type[StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE + MAX_ALLOWABLE_CURRENT = 20000 # mA + + # Gait State details + HS_GYRO_THRESHOLD: float = 100 + HS_GYRO_FILTER_N: int = 2 + HS_GYRO_FILTER_WN: float = 3 + HS_GYRO_DELAY: float = 0.05 + SWING_SLACK: int = 10000 + TOE_OFF_FRACTION: float = 0.62 + REEL_IN_TIMEOUT: float = 0.2 + + # 4 point Spline + RISE_FRACTION: float = 0.2 + PEAK_FRACTION: float = 0.53 + FALL_FRACTION: float = 0.63 + PEAK_TORQUE: float = 5 + SPLINE_BIAS: float = 3 # Nm + + # Impedance + K_VAL: int = 500 + B_VAL: int = 0 + SET_POINT: float = 0 # Deg + + READ_ONLY = False # Does not require Lipos + DO_READ_FSRS = False + + PRINT_HS = True # Print heel strikes + SLIP_DETECT_ACTIVE = False +''' diff --git a/exoboot.py b/exoboot.py index 35cbab9..ec6efcd 100644 --- a/exoboot.py +++ b/exoboot.py @@ -320,7 +320,7 @@ def read_data(self, self.data.gyro_y = -1 * self.motor_sign * \ actpack_data.gyroy * constants.GYRO_GAIN #Remove -1 for EB-51 - self.data.gyro_z = self.motor_sign * actpack_data.gyroz * constants.GYRO_GAIN # sign may be different from Max's device + self.data.gyro_z = -1 * self.motor_sign * actpack_data.gyroz * constants.GYRO_GAIN # sign may be different from Max's device '''Motor angle and current are kept in Dephy's orientation, but ankle angle and torque are converted to positive = plantarflexion.''' self.data.motor_angle = actpack_data.mot_ang From f6a8f00cbd8e449221009063b6b12e60b8504c1f Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Fri, 22 Apr 2022 08:54:32 +0100 Subject: [PATCH 03/12] changes --- config_util.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/config_util.py b/config_util.py index 5817243..1d666b2 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.075 #0.2 - PEAK_FRACTION: float = 0.33535#0.53 - FALL_FRACTION: float = 0.44478#0.60 - PEAK_TORQUE: float = 18.96235#5 + RISE_FRACTION: float = 0.2 + PEAK_FRACTION: float = 0.53 + FALL_FRACTION: float = 0.60 + PEAK_TORQUE: float = 5 SPLINE_BIAS: float = 3 # Nm # Impedance From eca5180ca0f6b048b28d2bae6005e14775584be4 Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Fri, 22 Apr 2022 11:01:56 +0100 Subject: [PATCH 04/12] update --- config_util.py | 8 ++++---- constants.py | 10 ++++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/config_util.py b/config_util.py index 1d666b2..5817243 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.075 #0.2 + PEAK_FRACTION: float = 0.33535#0.53 + FALL_FRACTION: float = 0.44478#0.60 + PEAK_TORQUE: float = 18.96235#5 SPLINE_BIAS: float = 3 # Nm # Impedance diff --git a/constants.py b/constants.py index 9f65d4c..230f14a 100644 --- a/constants.py +++ b/constants.py @@ -20,14 +20,16 @@ class ExobootModel(Enum): MIN_ANKLE_ANGLE = -45 # degrees, dorsiflexion # These polynomials are derived from the calibration routine (calibrate.py), analyzed with transmission_analysis.py - LEFT_ANKLE_TO_MOTOR = [ + """LEFT_ANKLE_TO_MOTOR = [ 1.42930145e-05, 9.60847697e-04, 9.66033271e-03, 1.21997272e+00, -7.39600859e+02, -2.96580581e+04 - ] - RIGHT_ANKLE_TO_MOTOR = [ + ]""" + LEFT_ANKLE_TO_MOTOR = [ 4.04430823e-06, 8.55008776e-04, 4.16160420e-02, 1.52798433e+00, -7.60189930e+02, -2.98125682e+04] + """RIGHT_ANKLE_TO_MOTOR = [ -1.02595964e-05, -9.28352253e-04, -3.10556810e-02, -1.40767218e+00, 7.57989378e+02, -3.48608044e+03 - ] + ]""" + RIGHT_ANKLE_TO_MOTOR = [-2.07317154e-06, -1.02471558e-03, -4.25686613e-02, -9.64798203e-01, 7.71135655e+02, 1.11783125e+04] # These points are used to create a Pchip spline, which defines the transmission ratio as a function of ankle angle ANKLE_PTS_LEFT = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, From 94dabcd82406ef288df0498a5d081bdb929ebd7e Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Tue, 26 Apr 2022 02:57:59 +0100 Subject: [PATCH 05/12] Latest --- Transmission.py | 70 ++++++++++++++++++++++++++++++++++++++ config_util.py | 12 ++++--- constants.py | 4 +-- custom_configs/test_config | 8 +++++ 4 files changed, 88 insertions(+), 6 deletions(-) create mode 100644 Transmission.py create mode 100644 custom_configs/test_config diff --git a/Transmission.py b/Transmission.py new file mode 100644 index 0000000..4416a4d --- /dev/null +++ b/Transmission.py @@ -0,0 +1,70 @@ +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 ["20220425_2352_calibration2_LEFT.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 + + + 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 ["20220425_2353_calibration2_RIGHT.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 + + + 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)) diff --git a/config_util.py b/config_util.py index 5817243..be786d2 100644 --- a/config_util.py +++ b/config_util.py @@ -68,10 +68,14 @@ class ConfigurableConstants(): SWING_ONLY: bool = False # 4 point Spline - RISE_FRACTION: float = 0.075 #0.2 - PEAK_FRACTION: float = 0.33535#0.53 - FALL_FRACTION: float = 0.44478#0.60 - PEAK_TORQUE: float = 18.96235#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# + """RISE_FRACTION: float = 0.376400032043457#0.2#0.075 + PEAK_FRACTION: float = 0.59497730255127#0.53#0.33535# + FALL_FRACTION: float = 0.65#0.60#0.44478# + PEAK_TORQUE: float = 17.2818012237549#5#18.96235#""" SPLINE_BIAS: float = 3 # Nm # Impedance diff --git a/constants.py b/constants.py index 230f14a..fca4916 100644 --- a/constants.py +++ b/constants.py @@ -42,8 +42,8 @@ class ExobootModel(Enum): TR_PTS_RIGHT = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5, -11]) # Nm/Nm - LEFT_ANKLE_ANGLE_OFFSET = 201 #-92 # 7, - RIGHT_ANKLE_ANGLE_OFFSET = -198# deg + LEFT_ANKLE_ANGLE_OFFSET = 199#201 #-92 # 7, + RIGHT_ANKLE_ANGLE_OFFSET = -200#-198# deg elif (ExoType == ExobootModel.EB51): diff --git a/custom_configs/test_config b/custom_configs/test_config new file mode 100644 index 0000000..da90ca1 --- /dev/null +++ b/custom_configs/test_config @@ -0,0 +1,8 @@ + +import config_util +config = config_util.ConfigurableConstants() + +config.RISE_FRACTION: float = 0.376400032043457#0.2#0.075 +config.PEAK_FRACTION: float = 0.59497730255127#0.53#0.33535# +config.FALL_FRACTION: float = 0.65#0.60#0.44478# +config.PEAK_TORQUE: float = 17.2818012237549#5#18.96235# \ No newline at end of file From 2cc5483266844a3e6cd17d7537f46eed1a766139 Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Tue, 3 May 2022 19:59:59 +0100 Subject: [PATCH 06/12] Latest --- Calibrate_Latest.py | 38 ++++++++++++++++++++++++++++++++++++++ Transmission.py | 8 ++++---- constants.py | 4 ++-- 3 files changed, 44 insertions(+), 6 deletions(-) create mode 100644 Calibrate_Latest.py diff --git a/Calibrate_Latest.py b/Calibrate_Latest.py new file mode 100644 index 0000000..8fdc74f --- /dev/null +++ b/Calibrate_Latest.py @@ -0,0 +1,38 @@ +import os +import sys +import exoboot +import time +import csv +import util +import constants +import config_util +import numpy as np + +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, + Kd=constants.DEFAULT_KD, ff=constants.DEFAULT_FF) + # exo.command_current(exo.motor_sign*1000) + print('begin!') + temp_ankle_angle_array = [] + for _ in range(750): + exo.command_current(exo.motor_sign*1000) + time.sleep(0.02) + exo.read_data(c) + temp_ankle_angle_array.append(exo.data.ankle_angle) + print("ankle_angle",exo.data.ankle_angle) + exo.write_data(c,False) + print('Done! File saved.') + print("Mean_ankle_angle",np.mean(temp_ankle_angle_array)) + + +if __name__ == '__main__': + config = config_util.load_config_from_args() + exo_list = exoboot.connect_to_exos(file_ID='calibration2',config=config) + if len(exo_list) > 1: + raise ValueError("Just turn on one exo for calibration") + exo = exo_list[0] + exo.standing_calibration(config) + calibrate_encoder_to_ankle_conversion(config,exo=exo) + exo.close() \ No newline at end of file diff --git a/Transmission.py b/Transmission.py index 4416a4d..17f0cbf 100644 --- a/Transmission.py +++ b/Transmission.py @@ -9,7 +9,7 @@ folder = 'exo_data/' -for filename in ["20220425_2352_calibration2_LEFT.csv"]: +for filename in ["20220427_0138_calibration2_LEFT.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: @@ -33,13 +33,13 @@ temp_left = [] - for i in range(50,250): + for i in range(50,850): temp_left.append(ankle_angle[i]) #print(ankle_angle) print("Average Ankle Angle Left", np.mean(temp_left)) -for filename in ["20220425_2353_calibration2_RIGHT.csv"]: +for filename in ["20220427_0132_calibration2_RIGHT.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: @@ -63,7 +63,7 @@ temp_right = [] - for i in range(50,250): + for i in range(50,850): temp_right.append(ankle_angle[i]) #print(ankle_angle) diff --git a/constants.py b/constants.py index fca4916..1c3e5be 100644 --- a/constants.py +++ b/constants.py @@ -42,8 +42,8 @@ class ExobootModel(Enum): TR_PTS_RIGHT = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5, -11]) # Nm/Nm - LEFT_ANKLE_ANGLE_OFFSET = 199#201 #-92 # 7, - RIGHT_ANKLE_ANGLE_OFFSET = -200#-198# deg + LEFT_ANKLE_ANGLE_OFFSET = 198.5#201.5#201 #-92 # 7, + RIGHT_ANKLE_ANGLE_OFFSET = -204.5#-195.5#-198# deg elif (ExoType == ExobootModel.EB51): From 2ca02189c16019fe9d7d9c0119f52aea53ed0966 Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Thu, 5 May 2022 02:31:31 +0100 Subject: [PATCH 07/12] Patrick session --- constants.py | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/constants.py b/constants.py index 1c3e5be..0b25783 100644 --- a/constants.py +++ b/constants.py @@ -24,26 +24,32 @@ class ExobootModel(Enum): 1.42930145e-05, 9.60847697e-04, 9.66033271e-03, 1.21997272e+00, -7.39600859e+02, -2.96580581e+04 ]""" - LEFT_ANKLE_TO_MOTOR = [ 4.04430823e-06, 8.55008776e-04, 4.16160420e-02, 1.52798433e+00, -7.60189930e+02, -2.98125682e+04] + LEFT_ANKLE_TO_MOTOR =[-3.86292446e-07, 9.87137568e-04, 5.46736005e-02, 1.28446551e+00, -7.72594712e+02, 2.48046256e+03] + #LEFT_ANKLE_TO_MOTOR = [ 4.04430823e-06, 8.55008776e-04, 4.16160420e-02, 1.52798433e+00, -7.60189930e+02, -2.98125682e+04] #Old TRansmission curve before subject 09 """RIGHT_ANKLE_TO_MOTOR = [ -1.02595964e-05, -9.28352253e-04, -3.10556810e-02, -1.40767218e+00, 7.57989378e+02, -3.48608044e+03 ]""" - RIGHT_ANKLE_TO_MOTOR = [-2.07317154e-06, -1.02471558e-03, -4.25686613e-02, -9.64798203e-01, 7.71135655e+02, 1.11783125e+04] + RIGHT_ANKLE_TO_MOTOR = [-1.39129122e-06, -1.07898390e-03, -4.85073970e-02, -9.89240927e-01, 7.75903076e+02, 4.47861727e+04] + #RIGHT_ANKLE_TO_MOTOR = [-2.07317154e-06, -1.02471558e-03, -4.25686613e-02, -9.64798203e-01, 7.71135655e+02, 1.11783125e+04] #Old TRansmission curve before subject 09 # These points are used to create a Pchip spline, which defines the transmission ratio as a function of ankle angle - ANKLE_PTS_LEFT = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, + #old + """ANKLE_PTS_LEFT = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, 55]) # Deg TR_PTS_LEFT = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5, - -11]) # Nm/Nm + -11]) # Nm/Nm""" - ANKLE_PTS_RIGHT = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, - 55]) # Deg - TR_PTS_RIGHT = np.array([19, 17, 16.5, 15.5, 13.5, 10, 4, -1, -5, - -11]) # Nm/Nm + + ANKLE_PTS_LEFT = np.array([-40, -20, -10, 0, 10, 20, 30, 40, 45.6, 50, 55]) - LEFT_ANKLE_ANGLE_OFFSET = 198.5#201.5#201 #-92 # 7, - RIGHT_ANKLE_ANGLE_OFFSET = -204.5#-195.5#-198# deg + TR_PTS_LEFT = np.array([19, 17.37, 17.2, 17, 16.1, 13.7, 10, 3.7, -1, -5, -11]) + + ANKLE_PTS_RIGHT = np.array([-40, -20, -10, 0, 10, 20, 30, 40, 45.6, 50, 55]) # Deg + TR_PTS_RIGHT = np.array([19, 17.37, 17.2, 17, 16.1, 13.7, 10, 3.7, -1, -5, -11])# Nm/Nm + + LEFT_ANKLE_ANGLE_OFFSET = 201#198.5#201.5#201 #-92 # 7, + RIGHT_ANKLE_ANGLE_OFFSET = -198.63#-204.5#-195.5#-198# deg elif (ExoType == ExobootModel.EB51): From 7ec3a03eee541478ece9708bb9c31ea8cd710ecd Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Thu, 5 May 2022 02:33:20 +0100 Subject: [PATCH 08/12] Patrick session --- Calibrate_Latest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Calibrate_Latest.py b/Calibrate_Latest.py index 8fdc74f..6317c4c 100644 --- a/Calibrate_Latest.py +++ b/Calibrate_Latest.py @@ -16,7 +16,7 @@ def calibrate_encoder_to_ankle_conversion(c,exo: exoboot.Exo): # exo.command_current(exo.motor_sign*1000) print('begin!') temp_ankle_angle_array = [] - for _ in range(750): + for _ in range(1000): exo.command_current(exo.motor_sign*1000) time.sleep(0.02) exo.read_data(c) From 6015103ac19e7d54065fbca79dd33959c82fe3cc Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Tue, 6 Jun 2023 20:26:10 +0100 Subject: [PATCH 09/12] Updated with continuous torque --- Calibrate_Latest.py | 2 +- ControllerCommunication.py | 83 +++++++ Message.proto | 81 +++++++ Message_pb2.py | 43 ++++ Message_pb2.pyi | 43 ++++ Message_pb2_grpc.py | 354 ++++++++++++++++++++++++++++++ config_util.py | 35 ++- constants.py | 8 +- constants_postprocessing.py | 1 + control_muxer.py | 6 + controllers.py | 73 +++++- custom_configs/default_config.py | 3 +- gait_state_estimators.py | 3 + main_loop_testing.py | 10 +- main_loop_varun.py | 149 +++++++++++++ optimization_main_loop.py | 3 +- state_machines.py | 2 + transmission_analysis_modified.py | 2 +- 18 files changed, 878 insertions(+), 23 deletions(-) create mode 100644 ControllerCommunication.py create mode 100644 Message.proto create mode 100644 Message_pb2.py create mode 100644 Message_pb2.pyi create mode 100644 Message_pb2_grpc.py create mode 100644 constants_postprocessing.py create mode 100644 main_loop_varun.py diff --git a/Calibrate_Latest.py b/Calibrate_Latest.py index 6317c4c..5fc5224 100644 --- a/Calibrate_Latest.py +++ b/Calibrate_Latest.py @@ -29,7 +29,7 @@ def calibrate_encoder_to_ankle_conversion(c,exo: exoboot.Exo): if __name__ == '__main__': config = config_util.load_config_from_args() - exo_list = exoboot.connect_to_exos(file_ID='calibration2',config=config) + exo_list = exoboot.connect_to_exos(file_ID='calibration2_2ndJune',config=config) if len(exo_list) > 1: raise ValueError("Just turn on one exo for calibration") exo = exo_list[0] diff --git a/ControllerCommunication.py b/ControllerCommunication.py new file mode 100644 index 0000000..86fca83 --- /dev/null +++ b/ControllerCommunication.py @@ -0,0 +1,83 @@ +import threading +from typing import Type + +from concurrent import futures +import grpc +import Message_pb2 +import Message_pb2_grpc + +import config_util +import exoboot +import time + +import asyncio + +class ControllerCommunication(threading.Thread): + def __init__(self,config: Type[config_util.ConfigurableConstants], exo_list, + new_params_event=Type[threading.Event], name = 'communication-thread'): + super().__init__(name=name) + self.daemon=True + self.config=config + self.new_params_event = new_params_event + self.exo_list = exo_list + self.stride_counter = 0 + self.temp_ankle_angle = [] + self.temp_ankle_angular_velocity = [] + #self.temp_counter = 0 + self.action_received = False + self.start() + + #def storing_the_stride_values(self): + + + def sending_data(self, ankle_a, ankle_v): + for exo in self.exo_list: + #print(exo.data.did_heel_strike, self.temp_ankle_angle, self.temp_ankle_angular_velocity) + """if (exo.data.did_heel_strike == True): + print("Here.............", self.stride_counter) + self.stride_counter += 1 + #if(self.stride_counter == 1 and self.stride_counter == 2): + self.temp_ankle_angle.append(exo.data.ankle_angle) + self.temp_ankle_angular_velocity.append(exo.data.ankle_velocity) + if(self.stride_counter == 2): + print("Sending values") + #print(exo.data.ankle_angle)""" + print("Sending values") + with grpc.insecure_channel( + self.config.CONTROLLER_ALGORITHM_COMMUNICATION, options=(('grpc.enable_http_proxy',0), )) as channel: + try: + print("Sending.............") + stub = Message_pb2_grpc.ControllerAlgorithmStub(channel) + response = stub.ControllerMessage( + Message_pb2.ControllerPing(ankle_angle = ankle_a, ankle_angular_velocity = ankle_v)) + print('Response', response) + #Message_pb2.ControllerPing(ankle_angle = exo.data.ankle_angle, ankle_angular_velocity = exo.data.ankle_velocity)) + except grpc.RpcError as e: + print("ERROR!!!!: ", e, "Check if the Computer IP is correct or if the computer side server is running") + + self.action_received = False + self.stride_counter = 0 + self.temp_ankle_angle = [] + self.temp_ankle_angular_velocity = [] + + class ActionState(Message_pb2_grpc.ActionStateServicer): + def __init__(self, ControllerCommunication): + self.ControllerCommunication = ControllerCommunication + + def ActionMessage(self,request,context): + print("Action received", request.action_torque_profile) + config_util.torque_profile = request.action_torque_profile + self.ControllerCommunication.action_received = True + return Message_pb2.Null() + + def server_to_receive_actions_for_the_controller(self): + print(".... Starting the server on the controller.....") + server = grpc.server(futures.ThreadPoolExecutor(max_workers=10)) + Message_pb2_grpc.add_ActionStateServicer_to_server(self.ActionState(self),server) + server.add_insecure_port(self.config.ALGORITHM_CONTROLLER_COMMUNICATION) + server.start() + server.wait_for_termination() + + def run(self): + #while True: + self.server_to_receive_actions_for_the_controller() \ No newline at end of file diff --git a/Message.proto b/Message.proto new file mode 100644 index 0000000..75e911f --- /dev/null +++ b/Message.proto @@ -0,0 +1,81 @@ +syntax = "proto3"; + +message Null {}; + +/* This service is between +GUI: Running on the Surface Tablet {Client} +and +Algorithm: Running on the computer {Server} +DESCRIPTION: +The GUI will send the user input {i.e int32 LIKE(1), DISLIKE(-1) or SKIP(0)} and will receive a +NULL message indicating new control parameters are generated -> new torque values +are given to the motor by the RL Algorithm +*/ +service GuiAlgorithm{ + rpc UserInput (GuiInput) returns (Null) {} + +} +enum EnumPreference{ + SKIP = 0; + LIKE = 1; + DISLIKE = -1; +} + +message GuiInput { + EnumPreference enumpreference = 1; +} + +/* This service is between +Algorithm: Running on the computer {Client} +and +GUI: Running on the Surface Tablet {Server} +DESCRIPTION: +The Algorithm will send commands to the GUI after "X" number of strides (X is a hyperparameter which has to be tuned) +to ask user's fro preference. +THe reason for this is because we want to do continous updates based on numeric rewards like every "Y" strides and only ask for +user preference after certain number of steps. Getting user preference is costly, so we want to continuauly learn based on numeric reward functions and +use user preference for torque profile tuning on a high level +*/ + +service AskingforPreference{ + rpc PreferenceUpdateStep (PreferenceFlag) returns (Null) {} +} + +message PreferenceFlag{ + bool PFlag = 1; +} + +/* +message PreferenceValue{ + EnumPreference enumpreference = 1; +} +*/ +/* This service is between +Lowlevel controlller: Running on the Rpi {Client} +and +Algorithm: Running on the computer {Server} +DESCRIPTION: + +*/ +service ControllerAlgorithm{ + rpc ControllerMessage (ControllerPing) returns (Null) {} +} + +message ControllerPing{ + repeated float ankle_angle = 1; + repeated float ankle_angular_velocity = 2; +} + +/* This service is between +Algorithm: Running on the computer {Client} +Lowlevel controlller: Running on the Rpi {Server} +DESCRIPTION: + +*/ +service ActionState{ + rpc ActionMessage (SendingAction) returns (Null) {} +} + +message SendingAction{ + repeated float action_torque_profile = 1; +} \ No newline at end of file diff --git a/Message_pb2.py b/Message_pb2.py new file mode 100644 index 0000000..1a8c5b1 --- /dev/null +++ b/Message_pb2.py @@ -0,0 +1,43 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: Message.proto +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rMessage.proto\"\x06\n\x04Null\"3\n\x08GuiInput\x12\'\n\x0e\x65numpreference\x18\x01 \x01(\x0e\x32\x0f.EnumPreference\"\x1f\n\x0ePreferenceFlag\x12\r\n\x05PFlag\x18\x01 \x01(\x08\"E\n\x0e\x43ontrollerPing\x12\x13\n\x0b\x61nkle_angle\x18\x01 \x03(\x02\x12\x1e\n\x16\x61nkle_angular_velocity\x18\x02 \x03(\x02\".\n\rSendingAction\x12\x1d\n\x15\x61\x63tion_torque_profile\x18\x01 \x03(\x02*:\n\x0e\x45numPreference\x12\x08\n\x04SKIP\x10\x00\x12\x08\n\x04LIKE\x10\x01\x12\x14\n\x07\x44ISLIKE\x10\xff\xff\xff\xff\xff\xff\xff\xff\xff\x01\x32/\n\x0cGuiAlgorithm\x12\x1f\n\tUserInput\x12\t.GuiInput\x1a\x05.Null\"\x00\x32G\n\x13\x41skingforPreference\x12\x30\n\x14PreferenceUpdateStep\x12\x0f.PreferenceFlag\x1a\x05.Null\"\x00\x32\x44\n\x13\x43ontrollerAlgorithm\x12-\n\x11\x43ontrollerMessage\x12\x0f.ControllerPing\x1a\x05.Null\"\x00\x32\x37\n\x0b\x41\x63tionState\x12(\n\rActionMessage\x12\x0e.SendingAction\x1a\x05.Null\"\x00\x62\x06proto3') + +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'Message_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _ENUMPREFERENCE._serialized_start=230 + _ENUMPREFERENCE._serialized_end=288 + _NULL._serialized_start=17 + _NULL._serialized_end=23 + _GUIINPUT._serialized_start=25 + _GUIINPUT._serialized_end=76 + _PREFERENCEFLAG._serialized_start=78 + _PREFERENCEFLAG._serialized_end=109 + _CONTROLLERPING._serialized_start=111 + _CONTROLLERPING._serialized_end=180 + _SENDINGACTION._serialized_start=182 + _SENDINGACTION._serialized_end=228 + _GUIALGORITHM._serialized_start=290 + _GUIALGORITHM._serialized_end=337 + _ASKINGFORPREFERENCE._serialized_start=339 + _ASKINGFORPREFERENCE._serialized_end=410 + _CONTROLLERALGORITHM._serialized_start=412 + _CONTROLLERALGORITHM._serialized_end=480 + _ACTIONSTATE._serialized_start=482 + _ACTIONSTATE._serialized_end=537 +# @@protoc_insertion_point(module_scope) diff --git a/Message_pb2.pyi b/Message_pb2.pyi new file mode 100644 index 0000000..482c19a --- /dev/null +++ b/Message_pb2.pyi @@ -0,0 +1,43 @@ +from google.protobuf.internal import containers as _containers +from google.protobuf.internal import enum_type_wrapper as _enum_type_wrapper +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from typing import ClassVar as _ClassVar, Iterable as _Iterable, Optional as _Optional, Union as _Union + +DESCRIPTOR: _descriptor.FileDescriptor +DISLIKE: EnumPreference +LIKE: EnumPreference +SKIP: EnumPreference + +class ControllerPing(_message.Message): + __slots__ = ["ankle_angle", "ankle_angular_velocity"] + ANKLE_ANGLE_FIELD_NUMBER: _ClassVar[int] + ANKLE_ANGULAR_VELOCITY_FIELD_NUMBER: _ClassVar[int] + ankle_angle: _containers.RepeatedScalarFieldContainer[float] + ankle_angular_velocity: _containers.RepeatedScalarFieldContainer[float] + def __init__(self, ankle_angle: _Optional[_Iterable[float]] = ..., ankle_angular_velocity: _Optional[_Iterable[float]] = ...) -> None: ... + +class GuiInput(_message.Message): + __slots__ = ["enumpreference"] + ENUMPREFERENCE_FIELD_NUMBER: _ClassVar[int] + enumpreference: EnumPreference + def __init__(self, enumpreference: _Optional[_Union[EnumPreference, str]] = ...) -> None: ... + +class Null(_message.Message): + __slots__ = [] + def __init__(self) -> None: ... + +class PreferenceFlag(_message.Message): + __slots__ = ["PFlag"] + PFLAG_FIELD_NUMBER: _ClassVar[int] + PFlag: bool + def __init__(self, PFlag: bool = ...) -> None: ... + +class SendingAction(_message.Message): + __slots__ = ["action_torque_profile"] + ACTION_TORQUE_PROFILE_FIELD_NUMBER: _ClassVar[int] + action_torque_profile: _containers.RepeatedScalarFieldContainer[float] + def __init__(self, action_torque_profile: _Optional[_Iterable[float]] = ...) -> None: ... + +class EnumPreference(int, metaclass=_enum_type_wrapper.EnumTypeWrapper): + __slots__ = [] diff --git a/Message_pb2_grpc.py b/Message_pb2_grpc.py new file mode 100644 index 0000000..8bbecb9 --- /dev/null +++ b/Message_pb2_grpc.py @@ -0,0 +1,354 @@ +# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT! +"""Client and server classes corresponding to protobuf-defined services.""" +import grpc + +import Message_pb2 as Message__pb2 + + +class GuiAlgorithmStub(object): + """This service is between + GUI: Running on the Surface Tablet {Client} + and + Algorithm: Running on the computer {Server} + DESCRIPTION: + The GUI will send the user input {i.e int32 LIKE(1), DISLIKE(-1) or SKIP(0)} and will receive a + NULL message indicating new control parameters are generated -> new torque values + are given to the motor by the RL Algorithm + """ + + def __init__(self, channel): + """Constructor. + + Args: + channel: A grpc.Channel. + """ + self.UserInput = channel.unary_unary( + '/GuiAlgorithm/UserInput', + request_serializer=Message__pb2.GuiInput.SerializeToString, + response_deserializer=Message__pb2.Null.FromString, + ) + + +class GuiAlgorithmServicer(object): + """This service is between + GUI: Running on the Surface Tablet {Client} + and + Algorithm: Running on the computer {Server} + DESCRIPTION: + The GUI will send the user input {i.e int32 LIKE(1), DISLIKE(-1) or SKIP(0)} and will receive a + NULL message indicating new control parameters are generated -> new torque values + are given to the motor by the RL Algorithm + """ + + def UserInput(self, request, context): + """Missing associated documentation comment in .proto file.""" + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + +def add_GuiAlgorithmServicer_to_server(servicer, server): + rpc_method_handlers = { + 'UserInput': grpc.unary_unary_rpc_method_handler( + servicer.UserInput, + request_deserializer=Message__pb2.GuiInput.FromString, + response_serializer=Message__pb2.Null.SerializeToString, + ), + } + generic_handler = grpc.method_handlers_generic_handler( + 'GuiAlgorithm', rpc_method_handlers) + server.add_generic_rpc_handlers((generic_handler,)) + + + # This class is part of an EXPERIMENTAL API. +class GuiAlgorithm(object): + """This service is between + GUI: Running on the Surface Tablet {Client} + and + Algorithm: Running on the computer {Server} + DESCRIPTION: + The GUI will send the user input {i.e int32 LIKE(1), DISLIKE(-1) or SKIP(0)} and will receive a + NULL message indicating new control parameters are generated -> new torque values + are given to the motor by the RL Algorithm + """ + + @staticmethod + def UserInput(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_unary(request, target, '/GuiAlgorithm/UserInput', + Message__pb2.GuiInput.SerializeToString, + Message__pb2.Null.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) + + +class AskingforPreferenceStub(object): + """This service is between + Algorithm: Running on the computer {Client} + and + GUI: Running on the Surface Tablet {Server} + DESCRIPTION: + The Algorithm will send commands to the GUI after "X" number of strides (X is a hyperparameter which has to be tuned) + to ask user's fro preference. + THe reason for this is because we want to do continous updates based on numeric rewards like every "Y" strides and only ask for + user preference after certain number of steps. Getting user preference is costly, so we want to continuauly learn based on numeric reward functions and + use user preference for torque profile tuning on a high level + + """ + + def __init__(self, channel): + """Constructor. + + Args: + channel: A grpc.Channel. + """ + self.PreferenceUpdateStep = channel.unary_unary( + '/AskingforPreference/PreferenceUpdateStep', + request_serializer=Message__pb2.PreferenceFlag.SerializeToString, + response_deserializer=Message__pb2.Null.FromString, + ) + + +class AskingforPreferenceServicer(object): + """This service is between + Algorithm: Running on the computer {Client} + and + GUI: Running on the Surface Tablet {Server} + DESCRIPTION: + The Algorithm will send commands to the GUI after "X" number of strides (X is a hyperparameter which has to be tuned) + to ask user's fro preference. + THe reason for this is because we want to do continous updates based on numeric rewards like every "Y" strides and only ask for + user preference after certain number of steps. Getting user preference is costly, so we want to continuauly learn based on numeric reward functions and + use user preference for torque profile tuning on a high level + + """ + + def PreferenceUpdateStep(self, request, context): + """Missing associated documentation comment in .proto file.""" + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + +def add_AskingforPreferenceServicer_to_server(servicer, server): + rpc_method_handlers = { + 'PreferenceUpdateStep': grpc.unary_unary_rpc_method_handler( + servicer.PreferenceUpdateStep, + request_deserializer=Message__pb2.PreferenceFlag.FromString, + response_serializer=Message__pb2.Null.SerializeToString, + ), + } + generic_handler = grpc.method_handlers_generic_handler( + 'AskingforPreference', rpc_method_handlers) + server.add_generic_rpc_handlers((generic_handler,)) + + + # This class is part of an EXPERIMENTAL API. +class AskingforPreference(object): + """This service is between + Algorithm: Running on the computer {Client} + and + GUI: Running on the Surface Tablet {Server} + DESCRIPTION: + The Algorithm will send commands to the GUI after "X" number of strides (X is a hyperparameter which has to be tuned) + to ask user's fro preference. + THe reason for this is because we want to do continous updates based on numeric rewards like every "Y" strides and only ask for + user preference after certain number of steps. Getting user preference is costly, so we want to continuauly learn based on numeric reward functions and + use user preference for torque profile tuning on a high level + + """ + + @staticmethod + def PreferenceUpdateStep(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_unary(request, target, '/AskingforPreference/PreferenceUpdateStep', + Message__pb2.PreferenceFlag.SerializeToString, + Message__pb2.Null.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) + + +class ControllerAlgorithmStub(object): + """ + message PreferenceValue{ + EnumPreference enumpreference = 1; + } + + This service is between + Lowlevel controlller: Running on the Rpi {Client} + and + Algorithm: Running on the computer {Server} + DESCRIPTION: + + """ + + def __init__(self, channel): + """Constructor. + + Args: + channel: A grpc.Channel. + """ + self.ControllerMessage = channel.unary_unary( + '/ControllerAlgorithm/ControllerMessage', + request_serializer=Message__pb2.ControllerPing.SerializeToString, + response_deserializer=Message__pb2.Null.FromString, + ) + + +class ControllerAlgorithmServicer(object): + """ + message PreferenceValue{ + EnumPreference enumpreference = 1; + } + + This service is between + Lowlevel controlller: Running on the Rpi {Client} + and + Algorithm: Running on the computer {Server} + DESCRIPTION: + + """ + + def ControllerMessage(self, request, context): + """Missing associated documentation comment in .proto file.""" + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + +def add_ControllerAlgorithmServicer_to_server(servicer, server): + rpc_method_handlers = { + 'ControllerMessage': grpc.unary_unary_rpc_method_handler( + servicer.ControllerMessage, + request_deserializer=Message__pb2.ControllerPing.FromString, + response_serializer=Message__pb2.Null.SerializeToString, + ), + } + generic_handler = grpc.method_handlers_generic_handler( + 'ControllerAlgorithm', rpc_method_handlers) + server.add_generic_rpc_handlers((generic_handler,)) + + + # This class is part of an EXPERIMENTAL API. +class ControllerAlgorithm(object): + """ + message PreferenceValue{ + EnumPreference enumpreference = 1; + } + + This service is between + Lowlevel controlller: Running on the Rpi {Client} + and + Algorithm: Running on the computer {Server} + DESCRIPTION: + + """ + + @staticmethod + def ControllerMessage(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_unary(request, target, '/ControllerAlgorithm/ControllerMessage', + Message__pb2.ControllerPing.SerializeToString, + Message__pb2.Null.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) + + +class ActionStateStub(object): + """This service is between + Algorithm: Running on the computer {Client} + Lowlevel controlller: Running on the Rpi {Server} + DESCRIPTION: + + """ + + def __init__(self, channel): + """Constructor. + + Args: + channel: A grpc.Channel. + """ + self.ActionMessage = channel.unary_unary( + '/ActionState/ActionMessage', + request_serializer=Message__pb2.SendingAction.SerializeToString, + response_deserializer=Message__pb2.Null.FromString, + ) + + +class ActionStateServicer(object): + """This service is between + Algorithm: Running on the computer {Client} + Lowlevel controlller: Running on the Rpi {Server} + DESCRIPTION: + + """ + + def ActionMessage(self, request, context): + """Missing associated documentation comment in .proto file.""" + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + +def add_ActionStateServicer_to_server(servicer, server): + rpc_method_handlers = { + 'ActionMessage': grpc.unary_unary_rpc_method_handler( + servicer.ActionMessage, + request_deserializer=Message__pb2.SendingAction.FromString, + response_serializer=Message__pb2.Null.SerializeToString, + ), + } + generic_handler = grpc.method_handlers_generic_handler( + 'ActionState', rpc_method_handlers) + server.add_generic_rpc_handlers((generic_handler,)) + + + # This class is part of an EXPERIMENTAL API. +class ActionState(object): + """This service is between + Algorithm: Running on the computer {Client} + Lowlevel controlller: Running on the Rpi {Server} + DESCRIPTION: + + """ + + @staticmethod + def ActionMessage(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_unary(request, target, '/ActionState/ActionMessage', + Message__pb2.SendingAction.SerializeToString, + Message__pb2.Null.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) diff --git a/config_util.py b/config_util.py index be786d2..624c643 100644 --- a/config_util.py +++ b/config_util.py @@ -9,6 +9,12 @@ import constants import os +torque_profile = [3.85632354298339, 2.64928505734501, 2.19254340731786, 2.28149527844568, 2.71153735627224, 3.58850718454041, 3.25824047087135, 3.43341900105777, 3.69050035822393, 4.07959414878489, 4.12148534296455, 4.25368163433673, 4.56492780355551, 4.07473982858473, 3.61169937058633, 3.35983841084969, 3.58537215572969, 4.07842383025961, 4.28982938400937, 4.16755596330112, \ + 4.54452112483794, 4.97528488658277, 4.26676647385884, 3.37354633916844, 3.02360513972130, 2.30604165976297, 1.52984653757886, 1.74176930399100, 1.84712208167220, 2.50812984428615, \ + 3.43102495449063, 3.69737030571121, 3.93090459695928, 4.55913434503884, 4.89341255088155, 5.54977390098578, 7.50194588598617, 8.87749955603490, 10.8893479232789, 12.7225871096851, 14.4387800922395, 15.8960544410945, 15,15, \ + 15,15,15,15,15,15,15,15,15,15,15,15,15,15, 14.0646346601844, 12.1609553291797, 9.83059826573726, 7.91142262288924, 6.07230704504248, 4.38679944145677, 3.92107328398523, 3.16633476032315, \ + 2.71278365547210, 3.06612670245022, 3.42706402420999, 3.64127420037985, 3.38843749160322, 3.77379709879310, 3.42936029267312, 3.68347083567085, 3.56301529751718, 3.23965841001272, 3.33620180630685, 3.53508733310225, 2.82684737780086, 2.29435135946539, 2.71326237932198, 2.54793579358701, 2.90572420095746, 2.83516616134697, 2.70566658759981, \ + 3.00729027577632, 3.46519682872717, 3.44015213540198, 4.15663336917760, 4.57301863598826, 5.16118518233301, 5.09956033769251, 4.09395357875526, 3.65748224695027, 3.18025085067750, 2.95722707993278, 2.69626025111302, 2.79078869617124, 3.37127760348953, 4.56819216145001] class Task(Enum): '''Used to determine gait_event_detector used and state machines used.''' @@ -21,11 +27,13 @@ class Task(Enum): class StanceCtrlStyle(Enum): '''Used to determine behavior during stance.''' - FOURPOINTSPLINE = 0 + #FOURPOINTSPLINE = 0 + VARUN = 0 GENERICSPLINE = 1 SAWICKIWICKI = 2 GENERICIMPEDANCE = 3 FIVEPOINTSPLINE = 4 + FOURPOINTSPLINE = 5 #* @dataclass @@ -50,8 +58,9 @@ class ConfigurableConstants(): ONLY_LOG_IF_NEW: bool = True TASK: Type[Task] = Task.WALKING - STANCE_CONTROL_STYLE: Type[ - StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE + """STANCE_CONTROL_STYLE: Type[ + StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE""" + STANCE_CONTROL_STYLE: Type[StanceCtrlStyle] = StanceCtrlStyle.VARUN MAX_ALLOWABLE_CURRENT = 20000 # mA # Gait State details @@ -68,10 +77,17 @@ class ConfigurableConstants(): SWING_ONLY: bool = False # 4 point Spline - RISE_FRACTION: float = 0.2#0.075 + #Original + """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# + PEAK_TORQUE: float = 5#18.96235#""" + + RISE_FRACTION: float = 0.2#0.075 + PEAK_FRACTION: float = 0.53#0.33535# + FALL_FRACTION: float = 0.65#0.44478# + PEAK_TORQUE: float = 8 + """RISE_FRACTION: float = 0.376400032043457#0.2#0.075 PEAK_FRACTION: float = 0.59497730255127#0.53#0.33535# FALL_FRACTION: float = 0.65#0.60#0.44478# @@ -110,6 +126,15 @@ class ConfigurableConstants(): resetting_time = 685 # the Exoboot typically stops updating after 731 seconds controller_stop_time = 710 + #Sending data using gRPC + sending_ankle_velocity = [] + sending_ankle_angle = [] + + #Receiving data from gRPC + #torque_profile = [] + #ip addresses + CONTROLLER_ALGORITHM_COMMUNICATION = f"{'141.212.77.28'}:" f"{'9090'}" + ALGORITHM_CONTROLLER_COMMUNICATION = f"{'67.194.39.92'}:" f"{'5050'}" class ConfigSaver(): diff --git a/constants.py b/constants.py index 0b25783..2ae6f87 100644 --- a/constants.py +++ b/constants.py @@ -9,8 +9,8 @@ class ExobootModel(Enum): EB51 = 1 -#ExoType: Type[ExobootModel] = ExobootModel.EB51 -ExoType: Type[ExobootModel] = ExobootModel.EB45 +ExoType: Type[ExobootModel] = ExobootModel.EB51 +#ExoType: Type[ExobootModel] = ExobootModel.EB45 DEFAULT_BAUD_RATE = 230400 TARGET_FREQ = 200 @@ -48,8 +48,8 @@ class ExobootModel(Enum): ANKLE_PTS_RIGHT = np.array([-40, -20, -10, 0, 10, 20, 30, 40, 45.6, 50, 55]) # Deg TR_PTS_RIGHT = np.array([19, 17.37, 17.2, 17, 16.1, 13.7, 10, 3.7, -1, -5, -11])# Nm/Nm - LEFT_ANKLE_ANGLE_OFFSET = 201#198.5#201.5#201 #-92 # 7, - RIGHT_ANKLE_ANGLE_OFFSET = -198.63#-204.5#-195.5#-198# deg + LEFT_ANKLE_ANGLE_OFFSET = 193#198.5#201.5#201 #-92 # 7, + RIGHT_ANKLE_ANGLE_OFFSET = -212.63#-204.5#-195.5#-198# deg elif (ExoType == ExobootModel.EB51): diff --git a/constants_postprocessing.py b/constants_postprocessing.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/constants_postprocessing.py @@ -0,0 +1 @@ + diff --git a/control_muxer.py b/control_muxer.py index c22889f..45898fe 100644 --- a/control_muxer.py +++ b/control_muxer.py @@ -58,6 +58,12 @@ def get_gse_and_sm_lists(exo_list, config: Type[config_util.ConfigurableConstant exo=exo, desired_slack=8000, force_timer_to_complete=True) stance_controller = controllers.SawickiWickiController( exo=exo, k_val=config.K_VAL, b_val=config.B_VAL) + elif config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.VARUN: + reel_out_controller = controllers.SoftReelOutController( + exo=exo, desired_slack=config.SWING_SLACK) + stance_controller = controllers.VarunContinuousTorqueController(exo=exo) + + state_machine = state_machines.StanceSwingReeloutReelinStateMachine(exo=exo, stance_controller=stance_controller, swing_controller=swing_controller, diff --git a/controllers.py b/controllers.py index 1c8d5c9..7192cfc 100644 --- a/controllers.py +++ b/controllers.py @@ -9,6 +9,8 @@ from collections import deque from typing import Type +import matplotlib.pyplot as plt +import numpy as np class Controller(object): '''Parent controller object. Child classes inherit methods.''' @@ -151,7 +153,6 @@ def command(self, reset=False): super().command_gains() self.exo.command_slack(desired_slack=self.desired_slack) - class GenericSplineController(Controller): def __init__(self, exo: Type[Exo], @@ -167,12 +168,17 @@ def __init__(self, self.spline = None # Placeholds so update_spline can fill self.last_spline self.update_spline(spline_x, spline_y, first_call=True) self.fade_duration = fade_duration + #* self.use_gait_phase = use_gait_phase # if False, use time (s) super().update_controller_gains(Kp=Kp, Ki=Ki, Kd=Kd, ff=ff) # Fade timer goes from 0 to fade_duration, active if below fade_duration (starts inactive) self.fade_start_time = time.perf_counter()-100 self.t0 = None + #* + self.temp_phase = [] + self.temp_torque = [] + def command(self, reset=False): '''Commands appropriate control. If reset=True, this controller was just switched to.''' if reset: @@ -197,6 +203,12 @@ def command(self, reset=False): phase=phase, fraction=(time.perf_counter()-self.fade_start_time)/self.fade_duration) else: desired_torque = self.spline(phase) + print(type(self.spline)) + #* + """self.temp_phase.append(phase) + self.temp_torque.append(desired_torque)""" + print("Phase", phase) + print("Desired_torque", desired_torque) self.exo.command_torque(desired_torque) def update_spline(self, spline_x, spline_y, first_call=False): @@ -208,14 +220,69 @@ def update_spline(self, spline_x, spline_y, first_call=False): self.last_spline = copy.deepcopy(self.spline) self.spline = interpolate.pchip( spline_x, spline_y, extrapolate=False) - def fade_splines(self, phase, fraction): torque_from_last_spline = self.last_spline(phase) torque_from_current_spline = self.spline(phase) desired_torque = (1-fraction)*torque_from_last_spline + \ fraction*torque_from_current_spline + #print("Desired_torque", desired_torque) return desired_torque +class VarunContinuousTorqueController(GenericSplineController): + def __init__(self,exo:Exo, + use_gait_phase: bool = True, + Kp: int = constants.DEFAULT_KP, + Ki: int = constants.DEFAULT_KI, + Kd: int = constants.DEFAULT_KD, + ff: int = constants.DEFAULT_FF): + + self.exo = Exo + self.spline = None + self.x_phase_value = np.linspace(0,1,100) + self.y_phase_value = config_util.torque_profile + self.use_gait_phase = use_gait_phase + print("Varun COntroller Running..............") + #super().update_controller_gains(Kp=Kp, Ki=Ki, Kd=Kd, ff=ff) + + super().__init__(exo=exo, + spline_x=self.x_phase_value, + spline_y = self.y_phase_value, + Kp=Kp, Ki=Ki, Kd=Kd, ff=ff, + fade_duration=5, + use_gait_phase=use_gait_phase) + + def command(self, reset= False): + '''Commands appropriate control. If reset=True, this controller was just switched to.''' + print("commanding the desired torque") + if reset: + super().command_gains() + self.t0 = time.perf_counter() + + if self.use_gait_phase: + phase = self.exo.data.gait_phase + else: + phase = time.perf_counter()-self.t0 + + if phase is None: + # Gait phase is sometimes None + desired_torque = 0 + elif phase > self.spline_x[-1]: + # If phase (elapsed time) is longer than spline is specified, use last spline point + print('phase is longer than specified spline') + desired_torque = self.spline(self.spline_x) + elif time.perf_counter() - self.fade_start_time < self.fade_duration: + # If fading splines + desired_torque = self.fade_splines( + phase=phase, fraction=(time.perf_counter()-self.fade_start_time)/self.fade_duration) + else: + desired_torque = self.spline(phase) + print(type(self.spline)) + #* + self.temp_phase.append(phase) + self.temp_torque.append(desired_torque) + print("Phase", self.temp_phase, len(self.temp_phase)) + print("Desired_torque", self.temp_torque, len(self.temp_torque)) + self.exo.command_torque(desired_torque) class FourPointSplineController(GenericSplineController): def __init__(self, @@ -242,7 +309,6 @@ def __init__(self, Kp=Kp, Ki=Ki, Kd=Kd, ff=ff, fade_duration=fade_duration, use_gait_phase=use_gait_phase) - def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants]): 'Updates controller parameters from the config object.''' super().update_spline(spline_x=self._get_spline_x(rise_fraction=config.RISE_FRACTION, @@ -262,7 +328,6 @@ def _get_spline_y(self, peak_torque) -> list: else: return [self.bias_torque, self.bias_torque, peak_torque, self.bias_torque, self.bias_torque] - class SmoothReelInController(Controller): def __init__(self, exo: Exo, diff --git a/custom_configs/default_config.py b/custom_configs/default_config.py index 1a78e60..13c3f7b 100644 --- a/custom_configs/default_config.py +++ b/custom_configs/default_config.py @@ -8,8 +8,7 @@ config.SWING_SLACK = 3500 #5000 config.TOE_OFF_FRACTION = 0.65 config.DO_INCLUDE_GEN_VARS = True - - +config.READ_ONLY = False ''' Here are the variables that are updatable in config, and their defaults: TARGET_FREQ: float = 200 # Hz diff --git a/gait_state_estimators.py b/gait_state_estimators.py index 5d1946c..7fcfb7f 100644 --- a/gait_state_estimators.py +++ b/gait_state_estimators.py @@ -36,6 +36,9 @@ def detect(self): if self.do_print_heel_strikes and data.did_heel_strike: print('heel strike detected on side: %-*s at time: %s' % (10, self.side, data.loop_time)) + return True #* + else:#* + return False#* #print("Enter detect did_heel_strike") def update_params_from_config(self, config: Type[config_util.ConfigurableConstants]): pass diff --git a/main_loop_testing.py b/main_loop_testing.py index 77b5c8f..705b1be 100644 --- a/main_loop_testing.py +++ b/main_loop_testing.py @@ -46,7 +46,7 @@ '''Perform standing calibration.''' if not config.READ_ONLY: for exo in exo_list: - standing_angle = exo.standing_calibration() + standing_angle = exo.standing_calibration(config) if exo.side == constants.Side.LEFT: config.LEFT_STANDING_ANGLE = standing_angle else: @@ -65,7 +65,7 @@ config_saver.write_data(loop_time=0) # Write first row on config only_write_if_new = not config.READ_ONLY and config.ONLY_LOG_IF_NEW for exo in exo_list: - exo.read_data() + exo.read_data(config) t0_state_time = exo.data.state_time print("ACTPACK_FREQUENCY",config.ACTPACK_FREQ) time.sleep(5) @@ -87,17 +87,17 @@ lock.release()""" for exo in exo_list: - exo.read_data(loop_time=loop_time) + exo.read_data(config, loop_time=loop_time) + print("Did heel strike", exo.data.did_heel_strike) for gait_state_estimator in gait_state_estimator_list: gait_state_estimator.detect() if not config.READ_ONLY: for state_machine in state_machine_list: state_machine.step(read_only=config.READ_ONLY) for exo in exo_list: - exo.write_data(only_write_if_new=only_write_if_new) + exo.write_data(config, only_write_if_new=only_write_if_new) if (int(loop_time % 20) == 0): - print("Loop TIme", loop_time) print("State_Time",exo.data.state_time - t0_state_time) diff --git a/main_loop_varun.py b/main_loop_varun.py new file mode 100644 index 0000000..3824238 --- /dev/null +++ b/main_loop_varun.py @@ -0,0 +1,149 @@ +import exoboot +import threading +import controllers +import state_machines +import gait_state_estimators +import constants +import filters +import time +import util +import config_util +import parameter_passers +import control_muxer +import plotters +import ml_util +import traceback + +import ControllerCommunication + +import scipy.signal +from scipy.signal import savgol_filter +import numpy as np +import matplotlib.pyplot as plt + + +config = config_util.load_config_from_args() # loads config from passed args +file_ID = input( + 'Other than the date, what would you like added to the filename?') + +'''if sync signal is used, this will be gpiozero object shared between exos.''' +sync_detector = config_util.get_sync_detector(config) + +'''Connect to Exos, instantiate Exo objects.''' +exo_list = exoboot.connect_to_exos( + file_ID=file_ID, config=config, sync_detector=sync_detector) +print('Battery Voltage: ', 0.001*exo_list[0].get_batt_voltage(), 'V') + +config_saver = config_util.ConfigSaver( + file_ID=file_ID, config=config) # Saves config updates + +'''Instantiate gait_state_estimator and state_machine objects, store in lists.''' +gait_state_estimator_list, state_machine_list = control_muxer.get_gse_and_sm_lists( + exo_list=exo_list, config=config) + +'''Prep parameter passing.''' +lock = threading.Lock() +quit_event = threading.Event() +new_params_event = threading.Event() +# v0.2,15,0.56,0.6! + +'''Perform standing calibration.''' +if not config.READ_ONLY: + for exo in exo_list: + standing_angle = exo.standing_calibration(config=config) + if exo.side == constants.Side.LEFT: + config.LEFT_STANDING_ANGLE = standing_angle + else: + config.RIGHT_STANDING_ANGLE = standing_angle +else: + print('Not calibrating... READ_ONLY = True in config') + +input('Press any key to begin') +print('Start!') + +'''Main Loop: Check param updates, Read data, calculate gait state, apply control, write data.''' +timer = util.FlexibleTimer( + target_freq=config.TARGET_FREQ) # attempts constants freq +t0 = time.perf_counter() +#keyboard_thread = parameter_passers.ParameterPasser(lock=lock, config=config, quit_event=quit_event,new_params_event=new_params_event) + +communication_thread = ControllerCommunication.ControllerCommunication(config=config, exo_list=exo_list, new_params_event = new_params_event) + +config_saver.write_data(loop_time=0) # Write first row on config +only_write_if_new = not config.READ_ONLY and config.ONLY_LOG_IF_NEW +for exo in exo_list: + exo.read_data(config) +t0_state_time = exo.data.state_time + +heel_strike_counter = 0 +t_1 = [] +t_2 = [] +temp_ankle_angle = [] +temp_ankle_angular_velocity = [] +while True: + try: + timer.pause() + loop_time = time.perf_counter() - t0 + + lock.acquire() + if new_params_event.is_set(): + config_saver.write_data(loop_time=loop_time) # Update config file + for state_machine in state_machine_list: # Make sure up to date + state_machine.update_ctrl_params_from_config(config=config) + for gait_state_estimator in gait_state_estimator_list: # Make sure up to date + gait_state_estimator.update_params_from_config(config=config) + new_params_event.clear() + if quit_event.is_set(): # If user enters "quit" + break + lock.release() + + for exo in exo_list: + exo.read_data(config=config,loop_time=loop_time) + for gait_state_estimator in gait_state_estimator_list: + gait_state_estimator.detect() + """t = gait_state_estimator.detect() + if(heel_strike_counter >= 1): + t_1.append(exo.data.ankle_angle) + t_2.append(exo.data.ankle_velocity) + if(t == True): #Uncomment for actual runnning + heel_strike_counter += 1 + if(heel_strike_counter % 2 == 0): + print(len(t_1)) + print(len(t_2)) + temp_ankle_angle.append(scipy.signal.resample(t_1,500)) #I have selected 500, I need to tune it!!!! + temp_ankle_angular_velocity.append(scipy.signal.resample(t_2, 500)) + t_1 = [] + t_2 = [] + if(heel_strike_counter % 10 == 0): + temp_ankle_angle = np.array(temp_ankle_angle).sum(axis=0) / 5 + temp_ankle_angular_velocity = np.array(temp_ankle_angular_velocity).sum(axis=0) / 5 + communication_thread.sending_data(temp_ankle_angle, temp_ankle_angular_velocity) #ToDo take care of two Exo + temp_ankle_angle = [] + temp_ankle_angular_velocity = [] + print("Outside the loop")""" + #print("further outside...") + if not config.READ_ONLY: + for state_machine in state_machine_list: + state_machine.step(read_only=config.READ_ONLY) + for exo in exo_list: + exo.write_data(config=config,only_write_if_new=only_write_if_new) + print("Exo torque", exo.data.ankle_torque_from_current) + + """if (int(loop_time % 20) == 0): + print("Loop TIme", loop_time) + print("State_Time",exo.data.state_time - t0_state_time)""" + #time.sleep(5) + """if(communication_thread.action_received == True): + print("Action received true")""" + """for exo in exo_list: + exo.sending_data(config)""" + """print(config.sending_ankle_angle) + print(config.sending_ankle_velocity) + time.sleep(0.2)""" + except KeyboardInterrupt: + print('Ctrl-C detected, Exiting Gracefully') + break + except Exception as err: + print(traceback.print_exc()) + print("Unexpected error:", err) + break \ No newline at end of file diff --git a/optimization_main_loop.py b/optimization_main_loop.py index b23013b..cb479bc 100644 --- a/optimization_main_loop.py +++ b/optimization_main_loop.py @@ -4,4 +4,5 @@ os.path.abspath(__file__))) + '/preference_learning_dephy_exo/online_optimization' print(pardir) sys.path.append(pardir) -import main_loop +#import main_loop +import main_loop_for_RL \ No newline at end of file diff --git a/state_machines.py b/state_machines.py index b3b085c..f3f193a 100644 --- a/state_machines.py +++ b/state_machines.py @@ -82,6 +82,7 @@ def __init__(self, self.controller_now = self.swing_controller def step(self, read_only=False): + print("Here.....................") # Check state machine transition criteria, switching controller_now if criteria are met if (self.controller_now == self.swing_controller and self.exo.data.did_heel_strike and @@ -156,6 +157,7 @@ def step(self, read_only=False): did_controllers_switch = False if not read_only: + #*self.controller_now.command(reset=did_controllers_switch) self.controller_now.command(reset=did_controllers_switch) def update_ctrl_params_from_config(self, config): diff --git a/transmission_analysis_modified.py b/transmission_analysis_modified.py index d7df32f..35fa8a6 100644 --- a/transmission_analysis_modified.py +++ b/transmission_analysis_modified.py @@ -9,7 +9,7 @@ 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 ["20230602_1845_calibration_2_2ndJUne_RIGHT.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: From bcb1e44587bb38b18399414f513af429384d7493 Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Mon, 26 Jun 2023 23:30:09 +0100 Subject: [PATCH 10/12] Continuous Torque controller working --- ControllerCommunication.py | 8 ++-- config_util.py | 33 ++++++++++---- constants.py | 22 +++++---- control_muxer.py | 11 ++--- controllers.py | 75 ++++++++++++++++++------------- custom_configs/default_config.py | 14 +++++- main_loop_varun.py | 15 ++++--- state_machines.py | 13 +++--- transmission_analysis.py | 2 +- transmission_analysis_modified.py | 2 +- 10 files changed, 117 insertions(+), 78 deletions(-) diff --git a/ControllerCommunication.py b/ControllerCommunication.py index 86fca83..84b9949 100644 --- a/ControllerCommunication.py +++ b/ControllerCommunication.py @@ -42,11 +42,11 @@ def sending_data(self, ankle_a, ankle_v): if(self.stride_counter == 2): print("Sending values") #print(exo.data.ankle_angle)""" - print("Sending values") + #print("Sending values") with grpc.insecure_channel( self.config.CONTROLLER_ALGORITHM_COMMUNICATION, options=(('grpc.enable_http_proxy',0), )) as channel: try: - print("Sending.............") + #print("Sending.............") stub = Message_pb2_grpc.ControllerAlgorithmStub(channel) response = stub.ControllerMessage( Message_pb2.ControllerPing(ankle_angle = ankle_a, ankle_angular_velocity = ankle_v)) @@ -65,8 +65,8 @@ def __init__(self, ControllerCommunication): self.ControllerCommunication = ControllerCommunication def ActionMessage(self,request,context): - print("Action received", request.action_torque_profile) - config_util.torque_profile = request.action_torque_profile + print("Action received")#, request.action_torque_profile) + self.ControllerCommunication.config.torque_profile= request.action_torque_profile self.ControllerCommunication.action_received = True return Message_pb2.Null() diff --git a/config_util.py b/config_util.py index 624c643..dc582d2 100644 --- a/config_util.py +++ b/config_util.py @@ -8,13 +8,25 @@ import argparse import constants import os - -torque_profile = [3.85632354298339, 2.64928505734501, 2.19254340731786, 2.28149527844568, 2.71153735627224, 3.58850718454041, 3.25824047087135, 3.43341900105777, 3.69050035822393, 4.07959414878489, 4.12148534296455, 4.25368163433673, 4.56492780355551, 4.07473982858473, 3.61169937058633, 3.35983841084969, 3.58537215572969, 4.07842383025961, 4.28982938400937, 4.16755596330112, \ - 4.54452112483794, 4.97528488658277, 4.26676647385884, 3.37354633916844, 3.02360513972130, 2.30604165976297, 1.52984653757886, 1.74176930399100, 1.84712208167220, 2.50812984428615, \ - 3.43102495449063, 3.69737030571121, 3.93090459695928, 4.55913434503884, 4.89341255088155, 5.54977390098578, 7.50194588598617, 8.87749955603490, 10.8893479232789, 12.7225871096851, 14.4387800922395, 15.8960544410945, 15,15, \ - 15,15,15,15,15,15,15,15,15,15,15,15,15,15, 14.0646346601844, 12.1609553291797, 9.83059826573726, 7.91142262288924, 6.07230704504248, 4.38679944145677, 3.92107328398523, 3.16633476032315, \ - 2.71278365547210, 3.06612670245022, 3.42706402420999, 3.64127420037985, 3.38843749160322, 3.77379709879310, 3.42936029267312, 3.68347083567085, 3.56301529751718, 3.23965841001272, 3.33620180630685, 3.53508733310225, 2.82684737780086, 2.29435135946539, 2.71326237932198, 2.54793579358701, 2.90572420095746, 2.83516616134697, 2.70566658759981, \ - 3.00729027577632, 3.46519682872717, 3.44015213540198, 4.15663336917760, 4.57301863598826, 5.16118518233301, 5.09956033769251, 4.09395357875526, 3.65748224695027, 3.18025085067750, 2.95722707993278, 2.69626025111302, 2.79078869617124, 3.37127760348953, 4.56819216145001] +import numpy as np + + +#torque_profile = np.zeros(100) +"""[8.288293838500977, 7.41025972366333, 6.616360187530518, 5.946132183074951, 5.439113616943359, 5.101890563964844, +5.2245354652404785, 5.757272243499756, 6.051341533660889, 6.692984104156494, 7.0892014503479, 7.113550662994385, 6.893687725067139, +6.587777614593506, 6.441828727722168, 6.371031761169434, 6.650856971740723, 6.529520511627197, 6.324128150939941, 6.16853666305542, +6.170462608337402, 6.544578552246094, 6.1557416915893555, 5.18583869934082, 4.360032558441162, 3.7516212463378906, 2.649479627609253, +2.313070297241211, 2.9656550884246826, 4.53773832321167, 6.539644241333008, 7.445051193237305, 8.095815658569336, 9.471872329711914, +7.275458812713623, 5.398955821990967, 4.238779067993164, 3.823606252670288, 3.944174289703369, 4.341067790985107, 5.743892192840576, + 7.426661968231201, 9.016322135925293, 7.285861968994141, 7.175071716308594, 7.154747486114502, 7.489355087280273, 7.804320812225342, + 8.24379825592041, 9.058961868286133, 8.922183990478516, 8.806507110595703, 8.346049308776855, 8.138028144836426, 7.960187911987305, + 8.917771339416504, 8.093070030212402, 5.960103988647461, 2.8361170291900635, -0.9650033116340637, -4.54809045791626, -8.64715576171875, + -9.511663436889648, -7.740748405456543, -3.54040789604187, -0.3012145757675171, 2.6928675174713135, 5.359005928039551, 6.8848371505737305, + 6.998980522155762, 5.680872917175293, 5.92738151550293, 5.7333197593688965, 5.684418201446533, 5.648083686828613, 5.666100978851318, + 5.766464710235596, 5.650455951690674, 5.576498985290527, 5.605972766876221, 5.67177677154541, 5.678019046783447, 5.949980735778809, + 6.067166805267334, 6.254060745239258, 6.255392074584961, 6.2189249992370605, 6.0487260818481445, 5.91334867477417, 5.913013935089111, + 5.928750514984131, 5.880381107330322, 5.9197611808776855, 6.0590057373046875, 5.938861846923828, 5.936643600463867, 5.9314727783203125, + 5.921031951904297, 5.900171756744385, 5.863742351531982]""" class Task(Enum): '''Used to determine gait_event_detector used and state machines used.''' @@ -94,6 +106,9 @@ class ConfigurableConstants(): PEAK_TORQUE: float = 17.2818012237549#5#18.96235#""" SPLINE_BIAS: float = 3 # Nm + #Varun Controller, Continuous torque + torque_profile = np.zeros(100) + # Impedance K_VAL: int = 500 B_VAL: int = 0 @@ -133,8 +148,8 @@ class ConfigurableConstants(): #Receiving data from gRPC #torque_profile = [] #ip addresses - CONTROLLER_ALGORITHM_COMMUNICATION = f"{'141.212.77.28'}:" f"{'9090'}" - ALGORITHM_CONTROLLER_COMMUNICATION = f"{'67.194.39.92'}:" f"{'5050'}" + CONTROLLER_ALGORITHM_COMMUNICATION = f"{'141.212.77.28'}:" f"{'7075'}" + ALGORITHM_CONTROLLER_COMMUNICATION = f"{'67.194.47.118'}:" f"{'5050'}" class ConfigSaver(): diff --git a/constants.py b/constants.py index 2ae6f87..3ddc7a0 100644 --- a/constants.py +++ b/constants.py @@ -30,7 +30,8 @@ class ExobootModel(Enum): -1.02595964e-05, -9.28352253e-04, -3.10556810e-02, -1.40767218e+00, 7.57989378e+02, -3.48608044e+03 ]""" - RIGHT_ANKLE_TO_MOTOR = [-1.39129122e-06, -1.07898390e-03, -4.85073970e-02, -9.89240927e-01, 7.75903076e+02, 4.47861727e+04] + RIGHT_ANKLE_TO_MOTOR = [ 1.99410790e-06, -7.30208995e-05, -5.20062767e-02, -2.00409178e+00, 5.34368291e+02, 3.18733045e+04] + #*[-1.39129122e-06, -1.07898390e-03, -4.85073970e-02, -9.89240927e-01, 7.75903076e+02, 4.47861727e+04] #RIGHT_ANKLE_TO_MOTOR = [-2.07317154e-06, -1.02471558e-03, -4.25686613e-02, -9.64798203e-01, 7.71135655e+02, 1.11783125e+04] #Old TRansmission curve before subject 09 # These points are used to create a Pchip spline, which defines the transmission ratio as a function of ankle angle @@ -58,28 +59,25 @@ class ExobootModel(Enum): MIN_ANKLE_ANGLE = -95 #EB-51 - #LEFT_ANKLE_TO_MOTOR = np.array([-2.27233618e-06, 3.55519545e-05, 4.73297332e-02, 3.18534948e+00,-5.63299330e+02, -2.85391796e+04]) - LEFT_ANKLE_TO_MOTOR = np.array([-2.41577803e-06, 6.03503507e-05, 4.69647778e-02, 3.14213596e+00,-5.63452288e+02, 2.04918050e+04]) - #LEFT_ANKLE_TO_MOTOR = np.array([-2.32052284e-06, 6.29683551e-05, 4.45431399e-02, 3.09556691e+00,-5.49941515e+02, -1.11211736e+04]) #used for the test trials + LEFT_ANKLE_TO_MOTOR = [-2.35289477e-06, 3.94977365e-04, 2.89480181e-02, -1.51262172e+00, -5.39358233e+02, -2.26523170e+04] + #used for preference LEFT_ANKLE_TO_MOTOR = np.array([-2.41577803e-06, 6.03503507e-05, 4.69647778e-02, 3.14213596e+00,-5.63452288e+02, 2.04918050e+04]) RIGHT_ANKLE_TO_MOTOR = np.array([5.05377935e-06, -2.83727629e-04, -7.31477078e-02, -1.56178494e+00, 6.34368537e+02, 9.35489733e+03]) #RIGHT_ANKLE_TO_MOTOR = np.array([ 5.37977952e-06, -2.76399833e-04, -7.58092528e-02, -1.65003833e+00,6.38860770e+02, 9.21647151e+03]) #used for the test trials #EB-51 - - ANKLE_PTS_LEFT = np.array([ + #* + ANKLE_PTS_LEFT = np.array([-67, -60, -47,-40, -20, -10 ,0, 5, 10, 20, 30, 40, 45.6, 55, 80, 90, 100, 112]) + TR_PTS_LEFT = np.array([13.91, 11.64, 9.3, 8.96, 10.18, 11.45, 12.13, 12.28, 12.2, 12.23, 11.4, 9.72, 8.76, 6.4, -2.54, -6.02, -10, -12.67]) + #Used for preference study + """ANKLE_PTS_LEFT = np.array([ -67, -60, -50, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 70, 80, 86 ]) TR_PTS_LEFT = 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 - ]) - #ANKLE_PTS_LEFT = np.array([-67, -60, -47, -40, -30, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 80, 90,100, 112]) - #TR_PTS_LEFT = 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]) - - #ANKLE_PTS_LEFT = np.array([-67, -60, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90, 100]) #used for the test trials - #TR_PTS_LEFT = 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]) #used for the test trials + ])""" ANKLE_PTS_RIGHT = np.array([ -67, -60, -50, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 70, 80, 86 diff --git a/control_muxer.py b/control_muxer.py index 45898fe..2000af8 100644 --- a/control_muxer.py +++ b/control_muxer.py @@ -41,9 +41,9 @@ def get_gse_and_sm_lists(exo_list, config: Type[config_util.ConfigurableConstant gait_state_estimator_list.append(gait_state_estimator) # Define State Machine - reel_in_controller = controllers.SmoothReelInController( + reel_in_controller = controllers.SmoothReelInController(config=config, exo=exo, reel_in_mV=config.REEL_IN_MV, slack_cutoff=config.REEL_IN_SLACK_CUTOFF, time_out=config.REEL_IN_TIMEOUT) - swing_controller = controllers.StalkController( + swing_controller = controllers.StalkController(config, exo=exo, desired_slack=config.SWING_SLACK) if config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.FOURPOINTSPLINE: reel_out_controller = controllers.SoftReelOutController( @@ -59,12 +59,13 @@ def get_gse_and_sm_lists(exo_list, config: Type[config_util.ConfigurableConstant stance_controller = controllers.SawickiWickiController( exo=exo, k_val=config.K_VAL, b_val=config.B_VAL) elif config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.VARUN: - reel_out_controller = controllers.SoftReelOutController( + print("Running Varun Continuous Torque controller") + reel_out_controller = controllers.SoftReelOutController(config, exo=exo, desired_slack=config.SWING_SLACK) - stance_controller = controllers.VarunContinuousTorqueController(exo=exo) + stance_controller = controllers.VarunContinuousTorqueController(config,exo=exo, torques=config.torque_profile) - state_machine = state_machines.StanceSwingReeloutReelinStateMachine(exo=exo, + state_machine = state_machines.StanceSwingReeloutReelinStateMachine(config=config,exo=exo, stance_controller=stance_controller, swing_controller=swing_controller, reel_in_controller=reel_in_controller, diff --git a/controllers.py b/controllers.py index 7192cfc..598d6d8 100644 --- a/controllers.py +++ b/controllers.py @@ -15,10 +15,11 @@ class Controller(object): '''Parent controller object. Child classes inherit methods.''' - def __init__(self, exo: Exo): + def __init__(self,config, exo: Exo): self.exo = exo + self.config= config - def command(self, reset): + def command(self, config,reset): '''For modularity, new controllers will ideally not take any arguments with their command() function. The exo object stored on self will have updated data, which is accessible to controller objects.''' @@ -137,7 +138,7 @@ def command(self, reset=False): class StalkController(Controller): - def __init__(self, + def __init__(self,config, exo: Exo, desired_slack: float, Kp: int = constants.DEFAULT_SWING_KP, @@ -148,13 +149,13 @@ def __init__(self, self.desired_slack = desired_slack super().update_controller_gains(Kp=Kp, Ki=Ki, Kd=Kd, ff=ff) - def command(self, reset=False): + def command(self, config, reset=False): if reset: super().command_gains() self.exo.command_slack(desired_slack=self.desired_slack) class GenericSplineController(Controller): - def __init__(self, + def __init__(self,config: Type[config_util.ConfigurableConstants], exo: Type[Exo], spline_x: list, spline_y: list, @@ -166,6 +167,8 @@ def __init__(self, ff: int = constants.DEFAULT_FF): self.exo = exo self.spline = None # Placeholds so update_spline can fill self.last_spline + spline_x = np.linspace(0,1,100) + spline_y = np.clip(config.torque_profile, 3, 20) self.update_spline(spline_x, spline_y, first_call=True) self.fade_duration = fade_duration #* @@ -179,7 +182,7 @@ def __init__(self, self.temp_phase = [] self.temp_torque = [] - def command(self, reset=False): + """def command(self, reset=False): '''Commands appropriate control. If reset=True, this controller was just switched to.''' if reset: super().command_gains() @@ -205,11 +208,9 @@ def command(self, reset=False): desired_torque = self.spline(phase) print(type(self.spline)) #* - """self.temp_phase.append(phase) - self.temp_torque.append(desired_torque)""" print("Phase", phase) print("Desired_torque", desired_torque) - self.exo.command_torque(desired_torque) + self.exo.command_torque(desired_torque)""" def update_spline(self, spline_x, spline_y, first_call=False): if first_call or self.spline_x != spline_x or self.spline_y != spline_y: @@ -229,31 +230,45 @@ def fade_splines(self, phase, fraction): return desired_torque class VarunContinuousTorqueController(GenericSplineController): - def __init__(self,exo:Exo, - use_gait_phase: bool = True, + def __init__(self, config: Type[config_util.ConfigurableConstants], + exo:Exo, + torques, Kp: int = constants.DEFAULT_KP, Ki: int = constants.DEFAULT_KI, Kd: int = constants.DEFAULT_KD, - ff: int = constants.DEFAULT_FF): + ff: int = constants.DEFAULT_FF, + fade_duration: float = 5, + use_gait_phase: bool = True): - self.exo = Exo + """self.exo = Exo self.spline = None + self.config = config""" + print("torque_received", torques) + #time.sleep(2) self.x_phase_value = np.linspace(0,1,100) - self.y_phase_value = config_util.torque_profile + self.y_phase_value = np.clip(torques, 3, 20) #ToDo figure out this clipping issue during the prediction stage + #self.spline = interpolate.pchip(self.x_phase_value, self.y_phase_value, extrapolate=False) self.use_gait_phase = use_gait_phase - print("Varun COntroller Running..............") + print("Varun Controller Running..............") #super().update_controller_gains(Kp=Kp, Ki=Ki, Kd=Kd, ff=ff) - - super().__init__(exo=exo, - spline_x=self.x_phase_value, - spline_y = self.y_phase_value, + super().__init__(config=config,exo=exo, + spline_x= np.linspace(0,1,100), + spline_y=np.clip(torques, 3, 20), Kp=Kp, Ki=Ki, Kd=Kd, ff=ff, - fade_duration=5, + fade_duration=fade_duration, use_gait_phase=use_gait_phase) + """def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants]): + self.spline = interpolate.pchip(self.x_phase_value, self.y_phase_value, extrapolate=False) """ + def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants]): + 'Updates controller parameters from the config object.''' + super().update_spline(spline_x=np.linspace(0,1,100), + spline_y=np.clip(config.torque_profile, 3, 20)) - def command(self, reset= False): + def command(self, config, reset= False): '''Commands appropriate control. If reset=True, this controller was just switched to.''' - print("commanding the desired torque") + #print("commanding the desired torque") + self.spline = interpolate.pchip(np.linspace(0,1,100), config.torque_profile, extrapolate=False) + #print(self.config.torque_profile) if reset: super().command_gains() self.t0 = time.perf_counter() @@ -276,13 +291,13 @@ def command(self, reset= False): phase=phase, fraction=(time.perf_counter()-self.fade_start_time)/self.fade_duration) else: desired_torque = self.spline(phase) - print(type(self.spline)) #* self.temp_phase.append(phase) self.temp_torque.append(desired_torque) - print("Phase", self.temp_phase, len(self.temp_phase)) - print("Desired_torque", self.temp_torque, len(self.temp_torque)) - self.exo.command_torque(desired_torque) + #print("Phase", self.temp_phase, len(self.temp_phase)) + print("Desired_torque")#,self.temp_torque) + desired_torque = np.clip(desired_torque,0,25) + self.exo.command_torque(desired_torque) class FourPointSplineController(GenericSplineController): def __init__(self, @@ -329,7 +344,7 @@ def _get_spline_y(self, peak_torque) -> list: return [self.bias_torque, self.bias_torque, peak_torque, self.bias_torque, self.bias_torque] class SmoothReelInController(Controller): - def __init__(self, + def __init__(self,config, exo: Exo, reel_in_mV: int = 1200, slack_cutoff: float = 1500, @@ -354,7 +369,7 @@ def __init__(self, self.delay_timer = util.DelayTimer(delay_time=time_out) self.reel_in_mV = reel_in_mV - def command(self, reset=False): + def command(self, config, reset=False): if reset: super().command_gains() self.delay_timer.start() @@ -408,7 +423,7 @@ def check_completion_status(self): class SoftReelOutController(Controller): - def __init__(self, + def __init__(self,config, exo: Exo, desired_slack: float = 7000, Kp: int = 100, @@ -425,7 +440,7 @@ def __init__(self, self.delay_timer = util.DelayTimer(delay_time=max_reel_out_time) self.force_timer_to_complete = force_timer_to_complete - def command(self, reset=False): + def command(self, config, reset=False): if reset: super().command_gains() self.delay_timer.start() diff --git a/custom_configs/default_config.py b/custom_configs/default_config.py index 13c3f7b..e822258 100644 --- a/custom_configs/default_config.py +++ b/custom_configs/default_config.py @@ -2,13 +2,23 @@ import config_util config = config_util.ConfigurableConstants() # config.HS_GYRO_DELAY = 0.05 # For example -config.MAX_ALLOWABLE_CURRENT = 25000 # mA -config.REEL_IN_MV = 900 +"""config.MAX_ALLOWABLE_CURRENT = 25000 # mA originally 25000 +config.REEL_IN_MV = 100 #* originally = 900 config.REEL_IN_TIMEOUT = 0.075 # 0.2 config.SWING_SLACK = 3500 #5000 +config.SWING_ONLY = False #* config.TOE_OFF_FRACTION = 0.65 +config.DO_INCLUDE_GEN_VARS = True""" +config.REEL_IN_MV = 100 +config.SWING_SLACK = 3000 config.DO_INCLUDE_GEN_VARS = True +config.SWING_ONLY = False +config.MAX_ALLOWABLE_CURRENT = 10000 # mA +config.PEAK_TORQUE = 22 +config.HS_GYRO_THRESHOLD = 10 config.READ_ONLY = False + +#* ''' Here are the variables that are updatable in config, and their defaults: TARGET_FREQ: float = 200 # Hz diff --git a/main_loop_varun.py b/main_loop_varun.py index 3824238..6f26390 100644 --- a/main_loop_varun.py +++ b/main_loop_varun.py @@ -100,8 +100,8 @@ for exo in exo_list: exo.read_data(config=config,loop_time=loop_time) for gait_state_estimator in gait_state_estimator_list: - gait_state_estimator.detect() - """t = gait_state_estimator.detect() + #gait_state_estimator.detect() + t = gait_state_estimator.detect() if(heel_strike_counter >= 1): t_1.append(exo.data.ankle_angle) t_2.append(exo.data.ankle_velocity) @@ -115,19 +115,20 @@ t_1 = [] t_2 = [] if(heel_strike_counter % 10 == 0): - temp_ankle_angle = np.array(temp_ankle_angle).sum(axis=0) / 5 - temp_ankle_angular_velocity = np.array(temp_ankle_angular_velocity).sum(axis=0) / 5 + temp_ankle_angle = np.array(temp_ankle_angle).sum(axis=0) / 9 + temp_ankle_angular_velocity = np.array(temp_ankle_angular_velocity).sum(axis=0) / 9 communication_thread.sending_data(temp_ankle_angle, temp_ankle_angular_velocity) #ToDo take care of two Exo temp_ankle_angle = [] temp_ankle_angular_velocity = [] - print("Outside the loop")""" + #print("Outside the loop") #print("further outside...") if not config.READ_ONLY: for state_machine in state_machine_list: - state_machine.step(read_only=config.READ_ONLY) + #print("Torque prolie",config.torque_profile) + state_machine.step(config = config, read_only=config.READ_ONLY) for exo in exo_list: exo.write_data(config=config,only_write_if_new=only_write_if_new) - print("Exo torque", exo.data.ankle_torque_from_current) + #print("Exo torque", exo.data.ankle_torque_from_current) """if (int(loop_time % 20) == 0): print("Loop TIme", loop_time) diff --git a/state_machines.py b/state_machines.py index f3f193a..709a858 100644 --- a/state_machines.py +++ b/state_machines.py @@ -13,11 +13,11 @@ class HighLevelController(): '''A class that steps through controllers depending on, for instance, gait events.''' - def __init__(self, + def __init__(self,config, exo: Type[Exo]): self.exo = exo - def step(self, read_only): + def step(self, config, read_only): '''Primary function to step through mid-level controllers.''' raise ValueError('step() not written yet for this controller') @@ -94,7 +94,6 @@ def step(self, read_only=False): did_controllers_switch = True else: did_controllers_switch = False - if not read_only: self.controller_now.command(reset=did_controllers_switch) @@ -105,7 +104,7 @@ def update_ctrl_params_from_config(self, config): class StanceSwingReeloutReelinStateMachine(HighLevelController): '''Unilateral state machine that takes in data, segments strides, and applies controllers''' - def __init__(self, + def __init__(self,config, exo: Exo, stance_controller: Type[controllers.Controller], swing_controller: Type[controllers.Controller], @@ -116,6 +115,7 @@ def __init__(self, '''A state machine object is associated with an exo, and reads/stores exo data, applies logic to determine gait states and phases, chooses the correct controllers, and applies the controller.''' + self.config = config self.exo = exo self.stance_controller = stance_controller self.swing_controller = swing_controller @@ -125,7 +125,7 @@ def __init__(self, self.just_starting = True self.swing_only = swing_only - def step(self, read_only=False): + def step(self, config,read_only=False): # Check state machine transition criteria, switching controller_now if criteria are met if self.just_starting: self.controller_now = self.reel_out_controller @@ -152,13 +152,12 @@ def step(self, read_only=False): self.controller_now = self.swing_controller did_controllers_switch = True self.exo.data.gen_var1 = 3 - else: did_controllers_switch = False if not read_only: #*self.controller_now.command(reset=did_controllers_switch) - self.controller_now.command(reset=did_controllers_switch) + self.controller_now.command(config=config,reset=did_controllers_switch) def update_ctrl_params_from_config(self, config): self.stance_controller.update_ctrl_params_from_config(config=config) diff --git a/transmission_analysis.py b/transmission_analysis.py index 4ba00db..a12fb40 100644 --- a/transmission_analysis.py +++ b/transmission_analysis.py @@ -14,7 +14,7 @@ 7.05016223e+02, -1.09811413e+04]) folder = 'exo_data/' -for filename in ["20211117_2240_calibration2_LEFT.csv"]: +for filename in ["20230614_1909_calibration2_LEFT.csv"]: # filename = "20210616_1945_calibration2_RIGHT.csv" with open(folder + filename) as f: motor_angle = [int(row["motor_angle"]) diff --git a/transmission_analysis_modified.py b/transmission_analysis_modified.py index 35fa8a6..ec17871 100644 --- a/transmission_analysis_modified.py +++ b/transmission_analysis_modified.py @@ -9,7 +9,7 @@ folder = 'exo_data/' # for filename in ["20220210_1440_calibration2_LEFT.csv"]: # for filename in ["20220216_1229_calibration2_LEFT.csv"]: -for filename in ["20230602_1845_calibration_2_2ndJUne_RIGHT.csv"]: +for filename in ["20230614_1909_calibration2_LEFT.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: From 22fe95643b52357198242f49b9fc706162a65b85 Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Thu, 6 Jul 2023 02:44:10 +0100 Subject: [PATCH 11/12] Update_both_exo --- ControllerCommunication.py | 13 +-- Message.proto | 1 + Message_pb2.py | 28 +++--- Message_pb2.pyi | 6 +- config_util.py | 6 +- constants.py | 4 +- controllers.py | 38 ++++---- custom_configs/default_config.py | 10 +-- exoboot.py | 2 +- main_loop_varun.py | 146 +++++++++++++++++++++++++------ state_machines.py | 4 +- 11 files changed, 176 insertions(+), 82 deletions(-) diff --git a/ControllerCommunication.py b/ControllerCommunication.py index 84b9949..9ac5c92 100644 --- a/ControllerCommunication.py +++ b/ControllerCommunication.py @@ -30,7 +30,7 @@ def __init__(self,config: Type[config_util.ConfigurableConstants], exo_list, #def storing_the_stride_values(self): - def sending_data(self, ankle_a, ankle_v): + def sending_data(self, ankle_a, ankle_v, exo_side): for exo in self.exo_list: #print(exo.data.did_heel_strike, self.temp_ankle_angle, self.temp_ankle_angular_velocity) """if (exo.data.did_heel_strike == True): @@ -46,11 +46,11 @@ def sending_data(self, ankle_a, ankle_v): with grpc.insecure_channel( self.config.CONTROLLER_ALGORITHM_COMMUNICATION, options=(('grpc.enable_http_proxy',0), )) as channel: try: - #print("Sending.............") + print("Sending.............") stub = Message_pb2_grpc.ControllerAlgorithmStub(channel) response = stub.ControllerMessage( - Message_pb2.ControllerPing(ankle_angle = ankle_a, ankle_angular_velocity = ankle_v)) - print('Response', response) + Message_pb2.ControllerPing(ankle_angle = ankle_a, ankle_angular_velocity = ankle_v, exo_side=exo_side)) + print('Response received') #Message_pb2.ControllerPing(ankle_angle = exo.data.ankle_angle, ankle_angular_velocity = exo.data.ankle_velocity)) except grpc.RpcError as e: print("ERROR!!!!: ", e, "Check if the Computer IP is correct or if the computer side server is running") @@ -66,8 +66,11 @@ def __init__(self, ControllerCommunication): def ActionMessage(self,request,context): print("Action received")#, request.action_torque_profile) - self.ControllerCommunication.config.torque_profile= request.action_torque_profile + self.ControllerCommunication.config.torque_profile = request.action_torque_profile + time.sleep(10) #Waiting for the action to be implemented on the exo self.ControllerCommunication.action_received = True + self.ControllerCommunication.config.action_received = True + self.ControllerCommunication.new_params_event.set() return Message_pb2.Null() def server_to_receive_actions_for_the_controller(self): diff --git a/Message.proto b/Message.proto index 75e911f..bca8e08 100644 --- a/Message.proto +++ b/Message.proto @@ -64,6 +64,7 @@ service ControllerAlgorithm{ message ControllerPing{ repeated float ankle_angle = 1; repeated float ankle_angular_velocity = 2; + float exo_side = 3; } /* This service is between diff --git a/Message_pb2.py b/Message_pb2.py index 1a8c5b1..f19ed3d 100644 --- a/Message_pb2.py +++ b/Message_pb2.py @@ -13,15 +13,15 @@ -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rMessage.proto\"\x06\n\x04Null\"3\n\x08GuiInput\x12\'\n\x0e\x65numpreference\x18\x01 \x01(\x0e\x32\x0f.EnumPreference\"\x1f\n\x0ePreferenceFlag\x12\r\n\x05PFlag\x18\x01 \x01(\x08\"E\n\x0e\x43ontrollerPing\x12\x13\n\x0b\x61nkle_angle\x18\x01 \x03(\x02\x12\x1e\n\x16\x61nkle_angular_velocity\x18\x02 \x03(\x02\".\n\rSendingAction\x12\x1d\n\x15\x61\x63tion_torque_profile\x18\x01 \x03(\x02*:\n\x0e\x45numPreference\x12\x08\n\x04SKIP\x10\x00\x12\x08\n\x04LIKE\x10\x01\x12\x14\n\x07\x44ISLIKE\x10\xff\xff\xff\xff\xff\xff\xff\xff\xff\x01\x32/\n\x0cGuiAlgorithm\x12\x1f\n\tUserInput\x12\t.GuiInput\x1a\x05.Null\"\x00\x32G\n\x13\x41skingforPreference\x12\x30\n\x14PreferenceUpdateStep\x12\x0f.PreferenceFlag\x1a\x05.Null\"\x00\x32\x44\n\x13\x43ontrollerAlgorithm\x12-\n\x11\x43ontrollerMessage\x12\x0f.ControllerPing\x1a\x05.Null\"\x00\x32\x37\n\x0b\x41\x63tionState\x12(\n\rActionMessage\x12\x0e.SendingAction\x1a\x05.Null\"\x00\x62\x06proto3') +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rMessage.proto\"\x06\n\x04Null\"3\n\x08GuiInput\x12\'\n\x0e\x65numpreference\x18\x01 \x01(\x0e\x32\x0f.EnumPreference\"\x1f\n\x0ePreferenceFlag\x12\r\n\x05PFlag\x18\x01 \x01(\x08\"W\n\x0e\x43ontrollerPing\x12\x13\n\x0b\x61nkle_angle\x18\x01 \x03(\x02\x12\x1e\n\x16\x61nkle_angular_velocity\x18\x02 \x03(\x02\x12\x10\n\x08\x65xo_side\x18\x03 \x01(\x02\".\n\rSendingAction\x12\x1d\n\x15\x61\x63tion_torque_profile\x18\x01 \x03(\x02*:\n\x0e\x45numPreference\x12\x08\n\x04SKIP\x10\x00\x12\x08\n\x04LIKE\x10\x01\x12\x14\n\x07\x44ISLIKE\x10\xff\xff\xff\xff\xff\xff\xff\xff\xff\x01\x32/\n\x0cGuiAlgorithm\x12\x1f\n\tUserInput\x12\t.GuiInput\x1a\x05.Null\"\x00\x32G\n\x13\x41skingforPreference\x12\x30\n\x14PreferenceUpdateStep\x12\x0f.PreferenceFlag\x1a\x05.Null\"\x00\x32\x44\n\x13\x43ontrollerAlgorithm\x12-\n\x11\x43ontrollerMessage\x12\x0f.ControllerPing\x1a\x05.Null\"\x00\x32\x37\n\x0b\x41\x63tionState\x12(\n\rActionMessage\x12\x0e.SendingAction\x1a\x05.Null\"\x00\x62\x06proto3') _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'Message_pb2', globals()) if _descriptor._USE_C_DESCRIPTORS == False: DESCRIPTOR._options = None - _ENUMPREFERENCE._serialized_start=230 - _ENUMPREFERENCE._serialized_end=288 + _ENUMPREFERENCE._serialized_start=248 + _ENUMPREFERENCE._serialized_end=306 _NULL._serialized_start=17 _NULL._serialized_end=23 _GUIINPUT._serialized_start=25 @@ -29,15 +29,15 @@ _PREFERENCEFLAG._serialized_start=78 _PREFERENCEFLAG._serialized_end=109 _CONTROLLERPING._serialized_start=111 - _CONTROLLERPING._serialized_end=180 - _SENDINGACTION._serialized_start=182 - _SENDINGACTION._serialized_end=228 - _GUIALGORITHM._serialized_start=290 - _GUIALGORITHM._serialized_end=337 - _ASKINGFORPREFERENCE._serialized_start=339 - _ASKINGFORPREFERENCE._serialized_end=410 - _CONTROLLERALGORITHM._serialized_start=412 - _CONTROLLERALGORITHM._serialized_end=480 - _ACTIONSTATE._serialized_start=482 - _ACTIONSTATE._serialized_end=537 + _CONTROLLERPING._serialized_end=198 + _SENDINGACTION._serialized_start=200 + _SENDINGACTION._serialized_end=246 + _GUIALGORITHM._serialized_start=308 + _GUIALGORITHM._serialized_end=355 + _ASKINGFORPREFERENCE._serialized_start=357 + _ASKINGFORPREFERENCE._serialized_end=428 + _CONTROLLERALGORITHM._serialized_start=430 + _CONTROLLERALGORITHM._serialized_end=498 + _ACTIONSTATE._serialized_start=500 + _ACTIONSTATE._serialized_end=555 # @@protoc_insertion_point(module_scope) diff --git a/Message_pb2.pyi b/Message_pb2.pyi index 482c19a..5ada83b 100644 --- a/Message_pb2.pyi +++ b/Message_pb2.pyi @@ -10,12 +10,14 @@ LIKE: EnumPreference SKIP: EnumPreference class ControllerPing(_message.Message): - __slots__ = ["ankle_angle", "ankle_angular_velocity"] + __slots__ = ["ankle_angle", "ankle_angular_velocity", "exo_side"] ANKLE_ANGLE_FIELD_NUMBER: _ClassVar[int] ANKLE_ANGULAR_VELOCITY_FIELD_NUMBER: _ClassVar[int] + EXO_SIDE_FIELD_NUMBER: _ClassVar[int] ankle_angle: _containers.RepeatedScalarFieldContainer[float] ankle_angular_velocity: _containers.RepeatedScalarFieldContainer[float] - def __init__(self, ankle_angle: _Optional[_Iterable[float]] = ..., ankle_angular_velocity: _Optional[_Iterable[float]] = ...) -> None: ... + exo_side: float + def __init__(self, ankle_angle: _Optional[_Iterable[float]] = ..., ankle_angular_velocity: _Optional[_Iterable[float]] = ..., exo_side: _Optional[float] = ...) -> None: ... class GuiInput(_message.Message): __slots__ = ["enumpreference"] diff --git a/config_util.py b/config_util.py index dc582d2..4265e55 100644 --- a/config_util.py +++ b/config_util.py @@ -70,8 +70,7 @@ class ConfigurableConstants(): ONLY_LOG_IF_NEW: bool = True TASK: Type[Task] = Task.WALKING - """STANCE_CONTROL_STYLE: Type[ - StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE""" + #STANCE_CONTROL_STYLE: Type[StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE STANCE_CONTROL_STYLE: Type[StanceCtrlStyle] = StanceCtrlStyle.VARUN MAX_ALLOWABLE_CURRENT = 20000 # mA @@ -98,7 +97,7 @@ class ConfigurableConstants(): RISE_FRACTION: float = 0.2#0.075 PEAK_FRACTION: float = 0.53#0.33535# FALL_FRACTION: float = 0.65#0.44478# - PEAK_TORQUE: float = 8 + PEAK_TORQUE: float = 12 """RISE_FRACTION: float = 0.376400032043457#0.2#0.075 PEAK_FRACTION: float = 0.59497730255127#0.53#0.33535# @@ -108,6 +107,7 @@ class ConfigurableConstants(): #Varun Controller, Continuous torque torque_profile = np.zeros(100) + action_received: float = False # Impedance K_VAL: int = 500 diff --git a/constants.py b/constants.py index 3ddc7a0..0993d5c 100644 --- a/constants.py +++ b/constants.py @@ -96,8 +96,8 @@ class ExobootModel(Enum): # -2.89416189e+01]) ## NEED TO CHANGE FOR RIGHT ANKLE #EB-67#-51 - LEFT_ANKLE_ANGLE_OFFSET = -67#-95#-67 # deg - RIGHT_ANKLE_ANGLE_OFFSET = 87.1 #100 # deg + LEFT_ANKLE_ANGLE_OFFSET = -80#*-67#-95#-67 # deg + RIGHT_ANKLE_ANGLE_OFFSET = 93.1#*87.1 #100 # deg # Add to these lists if dev_ids change, or new exos or actpacks are purchased! diff --git a/controllers.py b/controllers.py index 598d6d8..664f313 100644 --- a/controllers.py +++ b/controllers.py @@ -19,7 +19,7 @@ def __init__(self,config, exo: Exo): self.exo = exo self.config= config - def command(self, config,reset): + def command(self, config, reset): '''For modularity, new controllers will ideally not take any arguments with their command() function. The exo object stored on self will have updated data, which is accessible to controller objects.''' @@ -35,7 +35,7 @@ def update_controller_gains(self, Kp: int, Ki: int, Kd: int = 0, ff: int = 0): def command_gains(self): self.exo.update_gains(Kp=self.Kp, Ki=self.Ki, Kd=self.Kd, ff=self.ff) - def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants]): + def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants], torque): '''For modularity, new controllers ideally use this function to update internal control params (e.g., k_val, or rise_time) from the config object. If needed, add new ctrl params to ConfigurableConstants.''' @@ -168,7 +168,7 @@ def __init__(self,config: Type[config_util.ConfigurableConstants], self.exo = exo self.spline = None # Placeholds so update_spline can fill self.last_spline spline_x = np.linspace(0,1,100) - spline_y = np.clip(config.torque_profile, 3, 20) + spline_y = np.clip(config.torque_profile, 3, 25) self.update_spline(spline_x, spline_y, first_call=True) self.fade_duration = fade_duration #* @@ -182,8 +182,9 @@ def __init__(self,config: Type[config_util.ConfigurableConstants], self.temp_phase = [] self.temp_torque = [] - """def command(self, reset=False): + """def command(self, config ,reset=False): '''Commands appropriate control. If reset=True, this controller was just switched to.''' + self.spline = interpolate.pchip(np.linspace(0,1,100), config.torque_profile, extrapolate=False) if reset: super().command_gains() self.t0 = time.perf_counter() @@ -213,7 +214,8 @@ def __init__(self,config: Type[config_util.ConfigurableConstants], self.exo.command_torque(desired_torque)""" def update_spline(self, spline_x, spline_y, first_call=False): - if first_call or self.spline_x != spline_x or self.spline_y != spline_y: + if first_call or (np.array(self.spline_x) != np.array(spline_x)).any() or (np.array(self.spline_y) != np.array(spline_y)).any(): #For continuous torque controller + #if first_call or self.spline_x != spline_x or self.spline_y != spline_y: self.spline_x = spline_x self.spline_y = spline_y print('Splines updated: ', 'x = ', spline_x, 'y = ', spline_y) @@ -221,6 +223,7 @@ def update_spline(self, spline_x, spline_y, first_call=False): self.last_spline = copy.deepcopy(self.spline) self.spline = interpolate.pchip( spline_x, spline_y, extrapolate=False) + def fade_splines(self, phase, fraction): torque_from_last_spline = self.last_spline(phase) torque_from_current_spline = self.spline(phase) @@ -240,34 +243,33 @@ def __init__(self, config: Type[config_util.ConfigurableConstants], fade_duration: float = 5, use_gait_phase: bool = True): - """self.exo = Exo - self.spline = None - self.config = config""" + print("torque_received", torques) - #time.sleep(2) self.x_phase_value = np.linspace(0,1,100) - self.y_phase_value = np.clip(torques, 3, 20) #ToDo figure out this clipping issue during the prediction stage - #self.spline = interpolate.pchip(self.x_phase_value, self.y_phase_value, extrapolate=False) + self.y_phase_value = np.clip(torques, 3, 25) #ToDo figure out this clipping issue during the prediction stage + self.spline = None # Placeholds so update_spline can fill self.last_spline + self.spline = interpolate.pchip(np.linspace(0,1,100), config.torque_profile, extrapolate=False) + #self.update_spline(self.x_phase_value, self.y_phase_value, first_call=True) self.use_gait_phase = use_gait_phase print("Varun Controller Running..............") #super().update_controller_gains(Kp=Kp, Ki=Ki, Kd=Kd, ff=ff) super().__init__(config=config,exo=exo, spline_x= np.linspace(0,1,100), - spline_y=np.clip(torques, 3, 20), + spline_y= self.y_phase_value, Kp=Kp, Ki=Ki, Kd=Kd, ff=ff, fade_duration=fade_duration, use_gait_phase=use_gait_phase) """def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants]): self.spline = interpolate.pchip(self.x_phase_value, self.y_phase_value, extrapolate=False) """ - def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants]): + def update_ctrl_params_from_config(self, config: Type[config_util.ConfigurableConstants], torque): 'Updates controller parameters from the config object.''' - super().update_spline(spline_x=np.linspace(0,1,100), - spline_y=np.clip(config.torque_profile, 3, 20)) + super().update_spline(spline_x=np.linspace(0,1,100),spline_y=torque) + #super().update_spline(spline_x=[0,0.4,0.53,0.65,10],spline_y=[3,3,10,3,3]) #4point spline def command(self, config, reset= False): '''Commands appropriate control. If reset=True, this controller was just switched to.''' #print("commanding the desired torque") - self.spline = interpolate.pchip(np.linspace(0,1,100), config.torque_profile, extrapolate=False) + #self.spline = interpolate.pchip(np.linspace(0,1,100), config.torque_profile, extrapolate=False) #print(self.config.torque_profile) if reset: super().command_gains() @@ -295,8 +297,8 @@ def command(self, config, reset= False): self.temp_phase.append(phase) self.temp_torque.append(desired_torque) #print("Phase", self.temp_phase, len(self.temp_phase)) - print("Desired_torque")#,self.temp_torque) - desired_torque = np.clip(desired_torque,0,25) + #print("Desired_torque", phase,desired_torque)#, self.temp_torque) + desired_torque = np.clip(desired_torque,3,25) self.exo.command_torque(desired_torque) class FourPointSplineController(GenericSplineController): diff --git a/custom_configs/default_config.py b/custom_configs/default_config.py index e822258..80ad2fe 100644 --- a/custom_configs/default_config.py +++ b/custom_configs/default_config.py @@ -2,19 +2,13 @@ import config_util config = config_util.ConfigurableConstants() # config.HS_GYRO_DELAY = 0.05 # For example -"""config.MAX_ALLOWABLE_CURRENT = 25000 # mA originally 25000 -config.REEL_IN_MV = 100 #* originally = 900 +config.MAX_ALLOWABLE_CURRENT = 11000#25000 # mA originally 25000 +config.REEL_IN_MV = 900 config.REEL_IN_TIMEOUT = 0.075 # 0.2 config.SWING_SLACK = 3500 #5000 config.SWING_ONLY = False #* config.TOE_OFF_FRACTION = 0.65 -config.DO_INCLUDE_GEN_VARS = True""" -config.REEL_IN_MV = 100 -config.SWING_SLACK = 3000 config.DO_INCLUDE_GEN_VARS = True -config.SWING_ONLY = False -config.MAX_ALLOWABLE_CURRENT = 10000 # mA -config.PEAK_TORQUE = 22 config.HS_GYRO_THRESHOLD = 10 config.READ_ONLY = False diff --git a/exoboot.py b/exoboot.py index ec6efcd..46b6e47 100644 --- a/exoboot.py +++ b/exoboot.py @@ -320,7 +320,7 @@ def read_data(self, self.data.gyro_y = -1 * self.motor_sign * \ actpack_data.gyroy * constants.GYRO_GAIN #Remove -1 for EB-51 - self.data.gyro_z = -1 * self.motor_sign * actpack_data.gyroz * constants.GYRO_GAIN # sign may be different from Max's device + self.data.gyro_z = self.motor_sign * actpack_data.gyroz * constants.GYRO_GAIN#-1 * self.motor_sign * actpack_data.gyroz * constants.GYRO_GAIN # sign may be different from Max's device '''Motor angle and current are kept in Dephy's orientation, but ankle angle and torque are converted to positive = plantarflexion.''' self.data.motor_angle = actpack_data.mot_ang diff --git a/main_loop_varun.py b/main_loop_varun.py index 6f26390..1ea83d3 100644 --- a/main_loop_varun.py +++ b/main_loop_varun.py @@ -21,6 +21,9 @@ import numpy as np import matplotlib.pyplot as plt +#Real-time plotting +from rtplot import client +import math config = config_util.load_config_from_args() # loads config from passed args file_ID = input( @@ -75,16 +78,38 @@ exo.read_data(config) t0_state_time = exo.data.state_time -heel_strike_counter = 0 -t_1 = [] -t_2 = [] -temp_ankle_angle = [] -temp_ankle_angular_velocity = [] +heel_strike_counter_left = 0 +t_1_left = [] +t_2_left = [] +temp_ankle_angle_left = [] +temp_ankle_angular_velocity_left = [] + +heel_strike_counter_right = 0 +t_1_right = [] +t_2_right = [] +temp_ankle_angle_right = [] +temp_ankle_angular_velocity_right = [] + +angle_l = [] +angle_r = [] + +#ip address for the server -- RealTimePlotting +client.configure_ip('35.3.109.236')#("141.212.77.23") + +plot_1_configs = {'names': ['Ankle Angle Left'], 'title': "Ankle Angle Left", 'colors': ['r'], 'ylabel': "degrees", 'xlabel': 'timestep'} +plot_1_1_configs = {'names': ['Ankle Angle Right'], 'title': "Ankle Angle Right", 'colors': ['m'], 'ylabel': "degrees", 'xlabel': 'timestep'} +plot_2_configs = {'names': ['Ankle Torque'], 'title': "Ankle Torque", 'colors': ['g'], 'ylabel': "Nm", 'xlabel': 'timestep'} +plot_3_configs = {'names': ['Ankle velocity'], 'title': "Ankle Velocity", 'colors': ['b'], 'ylabel': "deg/sec", 'xlabel': 'timestep'} +plot_4_configs = {'names': ['Commanded Torque'], 'title': "Commanded Torque Exoboot", 'colors': ['y'], 'ylabel': "Nm", 'xlabel': 'timestep'} +All_plots = [plot_1_configs, plot_1_1_configs, plot_2_configs, plot_4_configs, plot_3_configs] +client.initialize_plots(All_plots) +#client.initialize_plots(["Commanded Torque"]) +plotting_counter = 0 +config.action_received = True while True: try: timer.pause() loop_time = time.perf_counter() - t0 - lock.acquire() if new_params_event.is_set(): config_saver.write_data(loop_time=loop_time) # Update config file @@ -96,38 +121,105 @@ if quit_event.is_set(): # If user enters "quit" break lock.release() - for exo in exo_list: exo.read_data(config=config,loop_time=loop_time) for gait_state_estimator in gait_state_estimator_list: #gait_state_estimator.detect() t = gait_state_estimator.detect() - if(heel_strike_counter >= 1): - t_1.append(exo.data.ankle_angle) - t_2.append(exo.data.ankle_velocity) - if(t == True): #Uncomment for actual runnning - heel_strike_counter += 1 - if(heel_strike_counter % 2 == 0): - print(len(t_1)) - print(len(t_2)) - temp_ankle_angle.append(scipy.signal.resample(t_1,500)) #I have selected 500, I need to tune it!!!! - temp_ankle_angular_velocity.append(scipy.signal.resample(t_2, 500)) - t_1 = [] - t_2 = [] - if(heel_strike_counter % 10 == 0): - temp_ankle_angle = np.array(temp_ankle_angle).sum(axis=0) / 9 - temp_ankle_angular_velocity = np.array(temp_ankle_angular_velocity).sum(axis=0) / 9 - communication_thread.sending_data(temp_ankle_angle, temp_ankle_angular_velocity) #ToDo take care of two Exo - temp_ankle_angle = [] - temp_ankle_angular_velocity = [] - #print("Outside the loop") + if(config.action_received == True and exo.side.value == 2): + #print("Left Exo") + if(heel_strike_counter_left >= 1): + #print("Appending the states...") + t_1_left.append(exo.data.ankle_angle) + t_2_left.append(exo.data.ankle_velocity) + if(t == True): #Uncomment for actual runnning + heel_strike_counter_left += 1 + if(heel_strike_counter_left % 2 == 0): + print(len(t_1_left)) + print(len(t_2_left)) + temp_ankle_angle_left.append(scipy.signal.resample(t_1_left,500)) #I have selected 500, I need to tune it!!!! + temp_ankle_angular_velocity_left.append(scipy.signal.resample(t_2_left, 500)) + t_1_left = [] + t_2_left = [] + if(heel_strike_counter_left % 9 == 0): + print("length",len(temp_ankle_angle_left)) + temp_ankle_angle_left = np.delete(temp_ankle_angle_left, 0, 0) #Removing the first two steps data, as it takes a few steps for the exo to activate the desired torque + temp_ankle_angle_left = np.delete(temp_ankle_angle_left, 0, 0) + + temp_ankle_angle_left = np.array(temp_ankle_angle_left).sum(axis=0) / 2 + temp_ankle_angular_velocity_left = np.delete(temp_ankle_angular_velocity_left, 0, 0) + temp_ankle_angular_velocity_left = np.delete(temp_ankle_angular_velocity_left, 0, 0) + + temp_ankle_angular_velocity_left = np.array(temp_ankle_angular_velocity_left).sum(axis=0) / 2 + communication_thread.sending_data(temp_ankle_angle_left, temp_ankle_angular_velocity_left, exo.side.value) #ToDo take care of two Exo + config.action_received = False + temp_ankle_angle_left = [] + temp_ankle_angular_velocity_left = [] + heel_strike_counter_left = 0 + #print("Outside the loop") + elif(config.action_received == True and exo.side.value == 1): + #print("Right Exo") + if(heel_strike_counter_right >= 1): + #print("Appending the states...") + t_1_right.append(exo.data.ankle_angle) + t_2_right.append(exo.data.ankle_velocity) + if(t == True): #Uncomment for actual runnning + heel_strike_counter_right += 1 + if(heel_strike_counter_right % 2 == 0): + print(len(t_1_right)) + print(len(t_2_right)) + temp_ankle_angle_right.append(scipy.signal.resample(t_1_right,500)) #I have selected 500, I need to tune it!!!! + temp_ankle_angular_velocity_right.append(scipy.signal.resample(t_2_right, 500)) + t_1_right = [] + t_2_right = [] + if(heel_strike_counter_right % 9 == 0): + print("length",len(temp_ankle_angle_right)) + temp_ankle_angle_right = np.delete(temp_ankle_angle_right, 0, 0) #Removing the first two steps data, as it takes a few steps for the exo to activate the desired torque + temp_ankle_angle_right = np.delete(temp_ankle_angle_right, 0, 0) + + temp_ankle_angle_right = np.array(temp_ankle_angle_right).sum(axis=0) / 2 + temp_ankle_angular_velocity_right = np.delete(temp_ankle_angular_velocity_right, 0, 0) + temp_ankle_angular_velocity_right = np.delete(temp_ankle_angular_velocity_right, 0, 0) + + temp_ankle_angular_velocity_right = np.array(temp_ankle_angular_velocity_right).sum(axis=0) / 2 + communication_thread.sending_data(temp_ankle_angle_right, temp_ankle_angular_velocity_right, exo.side.value) #ToDo take care of two Exo + config.action_received = False + temp_ankle_angle_right = [] + temp_ankle_angular_velocity_right = [] + heel_strike_counter_right = 0 + #print("Outside the loop") + else: + pass #print("further outside...") if not config.READ_ONLY: for state_machine in state_machine_list: - #print("Torque prolie",config.torque_profile) + #print("Torque", config.torque_profile) state_machine.step(config = config, read_only=config.READ_ONLY) for exo in exo_list: exo.write_data(config=config,only_write_if_new=only_write_if_new) + #RealTimePlotting + exo.data.commanded_torque = np.asarray(exo.data.commanded_torque) + exo.data.commanded_torque = exo.data.commanded_torque.astype(float) + if(math.isnan(exo.data.commanded_torque)): + t_commanded = 0 + else: + t_commanded = exo.data.commanded_torque + + if(exo.side.value == 1): + #print('right') + angle_r = exo.data.ankle_angle + elif(exo.side.value == 2): + #print('Left') + angle_l = exo.data.ankle_angle + else: + pass + plotting_data = [angle_l, angle_r, exo.data.ankle_torque_from_current, t_commanded,exo.data.ankle_velocity] + plotting_counter += 1 + if(plotting_counter%6 == 0): + client.send_array(plotting_data) + """t_plotting = np.asarray(config.torque_profile) + t_plotting = t_plotting.astype(float) + client.send_array(t_plotting)""" #print("Exo torque", exo.data.ankle_torque_from_current) """if (int(loop_time % 20) == 0): diff --git a/state_machines.py b/state_machines.py index 709a858..d7886d9 100644 --- a/state_machines.py +++ b/state_machines.py @@ -157,10 +157,10 @@ def step(self, config,read_only=False): if not read_only: #*self.controller_now.command(reset=did_controllers_switch) - self.controller_now.command(config=config,reset=did_controllers_switch) + self.controller_now.command(config=config, reset=did_controllers_switch) def update_ctrl_params_from_config(self, config): - self.stance_controller.update_ctrl_params_from_config(config=config) + self.stance_controller.update_ctrl_params_from_config(config=config, torque=config.torque_profile) if self.swing_only != config.SWING_ONLY: self.swing_only = config.SWING_ONLY print('Updated swing only to: ', self.swing_only) From 6c3880b302e210ec7fb2208f20fa102110fcc064 Mon Sep 17 00:00:00 2001 From: VarunSatyadevShetty Date: Sat, 15 Jul 2023 00:17:37 +0100 Subject: [PATCH 12/12] Latest --- ControllerCommunication.py | 4 +- Message.proto | 7 +- Message_pb2.py | 30 ++++---- Message_pb2.pyi | 18 ++--- controllers.py | 2 +- main_loop_varun.py | 144 ++++++++++++++++++++----------------- 6 files changed, 112 insertions(+), 93 deletions(-) diff --git a/ControllerCommunication.py b/ControllerCommunication.py index 9ac5c92..587e315 100644 --- a/ControllerCommunication.py +++ b/ControllerCommunication.py @@ -30,7 +30,7 @@ def __init__(self,config: Type[config_util.ConfigurableConstants], exo_list, #def storing_the_stride_values(self): - def sending_data(self, ankle_a, ankle_v, exo_side): + def sending_data(self, ankle_a_l, ankle_v_l, ankle_a_r, ankle_v_r): for exo in self.exo_list: #print(exo.data.did_heel_strike, self.temp_ankle_angle, self.temp_ankle_angular_velocity) """if (exo.data.did_heel_strike == True): @@ -49,7 +49,7 @@ def sending_data(self, ankle_a, ankle_v, exo_side): print("Sending.............") stub = Message_pb2_grpc.ControllerAlgorithmStub(channel) response = stub.ControllerMessage( - Message_pb2.ControllerPing(ankle_angle = ankle_a, ankle_angular_velocity = ankle_v, exo_side=exo_side)) + Message_pb2.ControllerPing(ankle_angle_LEFT = ankle_a_l, ankle_angular_velocity_LEFT = ankle_v_l, ankle_angle_RIGHT = ankle_a_r, ankle_angular_velocity_RIGHT = ankle_v_r)) print('Response received') #Message_pb2.ControllerPing(ankle_angle = exo.data.ankle_angle, ankle_angular_velocity = exo.data.ankle_velocity)) except grpc.RpcError as e: diff --git a/Message.proto b/Message.proto index bca8e08..abea456 100644 --- a/Message.proto +++ b/Message.proto @@ -62,9 +62,10 @@ service ControllerAlgorithm{ } message ControllerPing{ - repeated float ankle_angle = 1; - repeated float ankle_angular_velocity = 2; - float exo_side = 3; + repeated float ankle_angle_LEFT = 1; + repeated float ankle_angular_velocity_LEFT = 2; + repeated float ankle_angle_RIGHT = 3; + repeated float ankle_angular_velocity_RIGHT = 4; } /* This service is between diff --git a/Message_pb2.py b/Message_pb2.py index f19ed3d..abe176e 100644 --- a/Message_pb2.py +++ b/Message_pb2.py @@ -13,31 +13,31 @@ -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rMessage.proto\"\x06\n\x04Null\"3\n\x08GuiInput\x12\'\n\x0e\x65numpreference\x18\x01 \x01(\x0e\x32\x0f.EnumPreference\"\x1f\n\x0ePreferenceFlag\x12\r\n\x05PFlag\x18\x01 \x01(\x08\"W\n\x0e\x43ontrollerPing\x12\x13\n\x0b\x61nkle_angle\x18\x01 \x03(\x02\x12\x1e\n\x16\x61nkle_angular_velocity\x18\x02 \x03(\x02\x12\x10\n\x08\x65xo_side\x18\x03 \x01(\x02\".\n\rSendingAction\x12\x1d\n\x15\x61\x63tion_torque_profile\x18\x01 \x03(\x02*:\n\x0e\x45numPreference\x12\x08\n\x04SKIP\x10\x00\x12\x08\n\x04LIKE\x10\x01\x12\x14\n\x07\x44ISLIKE\x10\xff\xff\xff\xff\xff\xff\xff\xff\xff\x01\x32/\n\x0cGuiAlgorithm\x12\x1f\n\tUserInput\x12\t.GuiInput\x1a\x05.Null\"\x00\x32G\n\x13\x41skingforPreference\x12\x30\n\x14PreferenceUpdateStep\x12\x0f.PreferenceFlag\x1a\x05.Null\"\x00\x32\x44\n\x13\x43ontrollerAlgorithm\x12-\n\x11\x43ontrollerMessage\x12\x0f.ControllerPing\x1a\x05.Null\"\x00\x32\x37\n\x0b\x41\x63tionState\x12(\n\rActionMessage\x12\x0e.SendingAction\x1a\x05.Null\"\x00\x62\x06proto3') +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rMessage.proto\"\x06\n\x04Null\"3\n\x08GuiInput\x12\'\n\x0e\x65numpreference\x18\x01 \x01(\x0e\x32\x0f.EnumPreference\"\x1f\n\x0ePreferenceFlag\x12\r\n\x05PFlag\x18\x01 \x01(\x08\"\x90\x01\n\x0e\x43ontrollerPing\x12\x18\n\x10\x61nkle_angle_LEFT\x18\x01 \x03(\x02\x12#\n\x1b\x61nkle_angular_velocity_LEFT\x18\x02 \x03(\x02\x12\x19\n\x11\x61nkle_angle_RIGHT\x18\x03 \x03(\x02\x12$\n\x1c\x61nkle_angular_velocity_RIGHT\x18\x04 \x03(\x02\".\n\rSendingAction\x12\x1d\n\x15\x61\x63tion_torque_profile\x18\x01 \x03(\x02*:\n\x0e\x45numPreference\x12\x08\n\x04SKIP\x10\x00\x12\x08\n\x04LIKE\x10\x01\x12\x14\n\x07\x44ISLIKE\x10\xff\xff\xff\xff\xff\xff\xff\xff\xff\x01\x32/\n\x0cGuiAlgorithm\x12\x1f\n\tUserInput\x12\t.GuiInput\x1a\x05.Null\"\x00\x32G\n\x13\x41skingforPreference\x12\x30\n\x14PreferenceUpdateStep\x12\x0f.PreferenceFlag\x1a\x05.Null\"\x00\x32\x44\n\x13\x43ontrollerAlgorithm\x12-\n\x11\x43ontrollerMessage\x12\x0f.ControllerPing\x1a\x05.Null\"\x00\x32\x37\n\x0b\x41\x63tionState\x12(\n\rActionMessage\x12\x0e.SendingAction\x1a\x05.Null\"\x00\x62\x06proto3') _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'Message_pb2', globals()) if _descriptor._USE_C_DESCRIPTORS == False: DESCRIPTOR._options = None - _ENUMPREFERENCE._serialized_start=248 - _ENUMPREFERENCE._serialized_end=306 + _ENUMPREFERENCE._serialized_start=306 + _ENUMPREFERENCE._serialized_end=364 _NULL._serialized_start=17 _NULL._serialized_end=23 _GUIINPUT._serialized_start=25 _GUIINPUT._serialized_end=76 _PREFERENCEFLAG._serialized_start=78 _PREFERENCEFLAG._serialized_end=109 - _CONTROLLERPING._serialized_start=111 - _CONTROLLERPING._serialized_end=198 - _SENDINGACTION._serialized_start=200 - _SENDINGACTION._serialized_end=246 - _GUIALGORITHM._serialized_start=308 - _GUIALGORITHM._serialized_end=355 - _ASKINGFORPREFERENCE._serialized_start=357 - _ASKINGFORPREFERENCE._serialized_end=428 - _CONTROLLERALGORITHM._serialized_start=430 - _CONTROLLERALGORITHM._serialized_end=498 - _ACTIONSTATE._serialized_start=500 - _ACTIONSTATE._serialized_end=555 + _CONTROLLERPING._serialized_start=112 + _CONTROLLERPING._serialized_end=256 + _SENDINGACTION._serialized_start=258 + _SENDINGACTION._serialized_end=304 + _GUIALGORITHM._serialized_start=366 + _GUIALGORITHM._serialized_end=413 + _ASKINGFORPREFERENCE._serialized_start=415 + _ASKINGFORPREFERENCE._serialized_end=486 + _CONTROLLERALGORITHM._serialized_start=488 + _CONTROLLERALGORITHM._serialized_end=556 + _ACTIONSTATE._serialized_start=558 + _ACTIONSTATE._serialized_end=613 # @@protoc_insertion_point(module_scope) diff --git a/Message_pb2.pyi b/Message_pb2.pyi index 5ada83b..34156ed 100644 --- a/Message_pb2.pyi +++ b/Message_pb2.pyi @@ -10,14 +10,16 @@ LIKE: EnumPreference SKIP: EnumPreference class ControllerPing(_message.Message): - __slots__ = ["ankle_angle", "ankle_angular_velocity", "exo_side"] - ANKLE_ANGLE_FIELD_NUMBER: _ClassVar[int] - ANKLE_ANGULAR_VELOCITY_FIELD_NUMBER: _ClassVar[int] - EXO_SIDE_FIELD_NUMBER: _ClassVar[int] - ankle_angle: _containers.RepeatedScalarFieldContainer[float] - ankle_angular_velocity: _containers.RepeatedScalarFieldContainer[float] - exo_side: float - def __init__(self, ankle_angle: _Optional[_Iterable[float]] = ..., ankle_angular_velocity: _Optional[_Iterable[float]] = ..., exo_side: _Optional[float] = ...) -> None: ... + __slots__ = ["ankle_angle_LEFT", "ankle_angle_RIGHT", "ankle_angular_velocity_LEFT", "ankle_angular_velocity_RIGHT"] + ANKLE_ANGLE_LEFT_FIELD_NUMBER: _ClassVar[int] + ANKLE_ANGLE_RIGHT_FIELD_NUMBER: _ClassVar[int] + ANKLE_ANGULAR_VELOCITY_LEFT_FIELD_NUMBER: _ClassVar[int] + ANKLE_ANGULAR_VELOCITY_RIGHT_FIELD_NUMBER: _ClassVar[int] + ankle_angle_LEFT: _containers.RepeatedScalarFieldContainer[float] + ankle_angle_RIGHT: _containers.RepeatedScalarFieldContainer[float] + ankle_angular_velocity_LEFT: _containers.RepeatedScalarFieldContainer[float] + ankle_angular_velocity_RIGHT: _containers.RepeatedScalarFieldContainer[float] + def __init__(self, ankle_angle_LEFT: _Optional[_Iterable[float]] = ..., ankle_angular_velocity_LEFT: _Optional[_Iterable[float]] = ..., ankle_angle_RIGHT: _Optional[_Iterable[float]] = ..., ankle_angular_velocity_RIGHT: _Optional[_Iterable[float]] = ...) -> None: ... class GuiInput(_message.Message): __slots__ = ["enumpreference"] diff --git a/controllers.py b/controllers.py index 664f313..c7f4eb1 100644 --- a/controllers.py +++ b/controllers.py @@ -297,7 +297,7 @@ def command(self, config, reset= False): self.temp_phase.append(phase) self.temp_torque.append(desired_torque) #print("Phase", self.temp_phase, len(self.temp_phase)) - #print("Desired_torque", phase,desired_torque)#, self.temp_torque) + print("Desired_torque", phase,desired_torque)#, self.temp_torque) desired_torque = np.clip(desired_torque,3,25) self.exo.command_torque(desired_torque) diff --git a/main_loop_varun.py b/main_loop_varun.py index 1ea83d3..5875190 100644 --- a/main_loop_varun.py +++ b/main_loop_varun.py @@ -90,6 +90,12 @@ temp_ankle_angle_right = [] temp_ankle_angular_velocity_right = [] +ankle_a_l = [] +ankle_v_l = [] +ankle_a_r = [] +ankle_v_r = [] + + angle_l = [] angle_r = [] @@ -102,9 +108,12 @@ plot_3_configs = {'names': ['Ankle velocity'], 'title': "Ankle Velocity", 'colors': ['b'], 'ylabel': "deg/sec", 'xlabel': 'timestep'} plot_4_configs = {'names': ['Commanded Torque'], 'title': "Commanded Torque Exoboot", 'colors': ['y'], 'ylabel': "Nm", 'xlabel': 'timestep'} All_plots = [plot_1_configs, plot_1_1_configs, plot_2_configs, plot_4_configs, plot_3_configs] +#All_plots = [plot_2_configs] client.initialize_plots(All_plots) #client.initialize_plots(["Commanded Torque"]) plotting_counter = 0 +left_exo_data_counter = 0 +right_exo_data_counter = 0 config.action_received = True while True: try: @@ -126,70 +135,76 @@ for gait_state_estimator in gait_state_estimator_list: #gait_state_estimator.detect() t = gait_state_estimator.detect() - if(config.action_received == True and exo.side.value == 2): - #print("Left Exo") - if(heel_strike_counter_left >= 1): - #print("Appending the states...") - t_1_left.append(exo.data.ankle_angle) - t_2_left.append(exo.data.ankle_velocity) - if(t == True): #Uncomment for actual runnning - heel_strike_counter_left += 1 - if(heel_strike_counter_left % 2 == 0): - print(len(t_1_left)) - print(len(t_2_left)) - temp_ankle_angle_left.append(scipy.signal.resample(t_1_left,500)) #I have selected 500, I need to tune it!!!! - temp_ankle_angular_velocity_left.append(scipy.signal.resample(t_2_left, 500)) - t_1_left = [] - t_2_left = [] - if(heel_strike_counter_left % 9 == 0): - print("length",len(temp_ankle_angle_left)) - temp_ankle_angle_left = np.delete(temp_ankle_angle_left, 0, 0) #Removing the first two steps data, as it takes a few steps for the exo to activate the desired torque - temp_ankle_angle_left = np.delete(temp_ankle_angle_left, 0, 0) - - temp_ankle_angle_left = np.array(temp_ankle_angle_left).sum(axis=0) / 2 - temp_ankle_angular_velocity_left = np.delete(temp_ankle_angular_velocity_left, 0, 0) - temp_ankle_angular_velocity_left = np.delete(temp_ankle_angular_velocity_left, 0, 0) - - temp_ankle_angular_velocity_left = np.array(temp_ankle_angular_velocity_left).sum(axis=0) / 2 - communication_thread.sending_data(temp_ankle_angle_left, temp_ankle_angular_velocity_left, exo.side.value) #ToDo take care of two Exo - config.action_received = False - temp_ankle_angle_left = [] - temp_ankle_angular_velocity_left = [] - heel_strike_counter_left = 0 - #print("Outside the loop") - elif(config.action_received == True and exo.side.value == 1): - #print("Right Exo") - if(heel_strike_counter_right >= 1): - #print("Appending the states...") - t_1_right.append(exo.data.ankle_angle) - t_2_right.append(exo.data.ankle_velocity) - if(t == True): #Uncomment for actual runnning - heel_strike_counter_right += 1 - if(heel_strike_counter_right % 2 == 0): - print(len(t_1_right)) - print(len(t_2_right)) - temp_ankle_angle_right.append(scipy.signal.resample(t_1_right,500)) #I have selected 500, I need to tune it!!!! - temp_ankle_angular_velocity_right.append(scipy.signal.resample(t_2_right, 500)) - t_1_right = [] - t_2_right = [] - if(heel_strike_counter_right % 9 == 0): - print("length",len(temp_ankle_angle_right)) - temp_ankle_angle_right = np.delete(temp_ankle_angle_right, 0, 0) #Removing the first two steps data, as it takes a few steps for the exo to activate the desired torque - temp_ankle_angle_right = np.delete(temp_ankle_angle_right, 0, 0) - - temp_ankle_angle_right = np.array(temp_ankle_angle_right).sum(axis=0) / 2 - temp_ankle_angular_velocity_right = np.delete(temp_ankle_angular_velocity_right, 0, 0) - temp_ankle_angular_velocity_right = np.delete(temp_ankle_angular_velocity_right, 0, 0) - - temp_ankle_angular_velocity_right = np.array(temp_ankle_angular_velocity_right).sum(axis=0) / 2 - communication_thread.sending_data(temp_ankle_angle_right, temp_ankle_angular_velocity_right, exo.side.value) #ToDo take care of two Exo - config.action_received = False - temp_ankle_angle_right = [] - temp_ankle_angular_velocity_right = [] - heel_strike_counter_right = 0 - #print("Outside the loop") - else: - pass + for exo in exo_list: + if(config.action_received == True and exo.side.value == 2): + #print("Left Exo") + if(heel_strike_counter_left >= 1): + #print("Appending the states...") + t_1_left.append(exo.data.ankle_angle) + t_2_left.append(exo.data.ankle_velocity) + if(t == True): #Uncomment for actual runnning + heel_strike_counter_left += 1 + if(heel_strike_counter_left % 2 == 0): + print(len(t_1_left)) + print(len(t_2_left)) + temp_ankle_angle_left.append(scipy.signal.resample(t_1_left,500)) #I have selected 500, I need to tune it!!!! + temp_ankle_angular_velocity_left.append(scipy.signal.resample(t_2_left, 500)) + t_1_left = [] + t_2_left = [] + if(heel_strike_counter_left % 9 == 0): + print("length",len(temp_ankle_angle_left)) + temp_ankle_angle_left = np.delete(temp_ankle_angle_left, 0, 0) #Removing the first two steps data, as it takes a few steps for the exo to activate the desired torque + temp_ankle_angle_left = np.delete(temp_ankle_angle_left, 0, 0) + + ankle_a_l = np.array(temp_ankle_angle_left).sum(axis=0) / 2 + temp_ankle_angular_velocity_left = np.delete(temp_ankle_angular_velocity_left, 0, 0) + temp_ankle_angular_velocity_left = np.delete(temp_ankle_angular_velocity_left, 0, 0) + + ankle_v_l = np.array(temp_ankle_angular_velocity_left).sum(axis=0) / 2 + #communication_thread.sending_data(temp_ankle_angle_left, temp_ankle_angular_velocity_left, exo.side.value) #ToDo take care of two Exo + #config.action_received = False + temp_ankle_angle_left = [] + temp_ankle_angular_velocity_left = [] + heel_strike_counter_left = 0 + left_exo_data_counter += 1 + + if(config.action_received == True and exo.side.value == 1): + #print("Right Exo") + if(heel_strike_counter_right >= 1): + #print("Appending the states...") + t_1_right.append(exo.data.ankle_angle) + t_2_right.append(exo.data.ankle_velocity) + if(t == True): #Uncomment for actual runnning + heel_strike_counter_right += 1 + if(heel_strike_counter_right % 2 == 0): + print(len(t_1_right)) + print(len(t_2_right)) + temp_ankle_angle_right.append(scipy.signal.resample(t_1_right,500)) #I have selected 500, I need to tune it!!!! + temp_ankle_angular_velocity_right.append(scipy.signal.resample(t_2_right, 500)) + t_1_right = [] + t_2_right = [] + if(heel_strike_counter_right % 9 == 0): + print("length",len(temp_ankle_angle_right)) + temp_ankle_angle_right = np.delete(temp_ankle_angle_right, 0, 0) #Removing the first two steps data, as it takes a few steps for the exo to activate the desired torque + temp_ankle_angle_right = np.delete(temp_ankle_angle_right, 0, 0) + + ankle_a_r = np.array(temp_ankle_angle_right).sum(axis=0) / 2 + temp_ankle_angular_velocity_right = np.delete(temp_ankle_angular_velocity_right, 0, 0) + temp_ankle_angular_velocity_right = np.delete(temp_ankle_angular_velocity_right, 0, 0) + + ankle_v_r = np.array(temp_ankle_angular_velocity_right).sum(axis=0) / 2 + #communication_thread.sending_data(temp_ankle_angle_right, temp_ankle_angular_velocity_right, exo.side.value) #ToDo take care of two Exo + #config.action_received = False + temp_ankle_angle_right = [] + temp_ankle_angular_velocity_right = [] + heel_strike_counter_right = 0 + right_exo_data_counter += 1 + + if(left_exo_data_counter != 0 and right_exo_data_counter != 0): + communication_thread.sending_data(ankle_a_l, ankle_v_l, ankle_a_r, ankle_v_r) + left_exo_data_counter = 0 + right_exo_data_counter = 0 + config.action_received = False #print("further outside...") if not config.READ_ONLY: for state_machine in state_machine_list: @@ -214,6 +229,7 @@ else: pass plotting_data = [angle_l, angle_r, exo.data.ankle_torque_from_current, t_commanded,exo.data.ankle_velocity] + #plotting_data = [exo.data.ankle_torque_from_current] plotting_counter += 1 if(plotting_counter%6 == 0): client.send_array(plotting_data)