def makeLDAClassifier(class_list): for i in class_list: if i == 0: # Build pooled covariance, assumes that no-movment is always involved pooled_cov = pce.get_var('COV' + str(i)).to_np_array() else: tmpVal = pce.get_var('COV' + str(i)).to_np_array() pooled_cov += tmpVal num_classes = np.shape(class_list) pooled_cov = pooled_cov / num_classes[0] inv_pooled_cov = np.linalg.inv( pooled_cov) # Find the pooled inverse covariance matrix inv_pooled_cov = np.array(inv_pooled_cov, np.float64, order='F') pce.set_var('INVPOOL', inv_pooled_cov) for i in class_list: mVal = pce.get_var('MN' + str(i)).to_np_array() tmpWg = np.dot(inv_pooled_cov, mVal.T) tmpCg = -0.5 * (mVal.dot(inv_pooled_cov).dot(mVal.T)) if i == 0: Wg = tmpWg Cg = tmpCg else: Wg = np.concatenate((Wg, tmpWg), axis=1) Cg = np.concatenate((Cg, tmpCg), axis=1) Wg = np.array(Wg, np.float64, order='F') Cg = np.array(Cg, np.float64, order='F') return (Wg, Cg)
def run(): #i = 1 daq = np.array(pce.get_var('DAQ_DATA').to_np_array()[0:numEMG, :], order='F').astype(float) ratio = pce.get_var('NOISE_SCALE') / 100 amp = pce.get_var('MVC').to_np_array() dc = 5 * np.ones(daq.shape[1]) daq = (daq / a2d) - dc noise = ratio * np.random.randn(daq.shape[1]) ch = int(pce.get_var('NOISE_CH')) print ratio if pce.get_var('NOISE') == 1: for i in range(0, numEMG): daq[i, :] = daq[i, :] + noise elif pce.get_var('NOISE') == 2: # SNR dependent noise # noise *= np.amax(amp[:,ch]) # print np.amax(amp[:,ch]) daq[ch, :] = daq[ch, :] + noise elif pce.get_var('NOISE') == 3: daq[ch, :] = np.zeros( daq.shape[1]) + eps * np.random.randn(daq.shape[1]) daq = (daq + dc) * a2d pce.set_var('DAQ_NOISY', daq.astype(float, order='F'))
def sendData(): global connection # Get class prediction (CLASS_EST) and whether a new class has just been trained (NEW_CLASS). class_out = pce.get_var('CLASS_EST') class_new = pce.get_var('NEW_CLASS') # Get proportional control information and format into comma deliminated string. Shape[1] of N_T is used to get the number of modes (typically 7). class_pc = ';'.join(['%.5f' % num2 for num2 in [pce.get_var('PROP_CONTROL')[num] for num in range(0, pce.get_var('N_T').to_np_array().shape[1])]]) # Get the total number of patterns (N_C), number of repetition (N_R), and temporary number of patterns (N_T). ntot = stringFormatter(pce.get_var('N_C').to_np_array(), '%d') nreps = stringFormatter(pce.get_var('N_R').to_np_array(), '%d') npats = stringFormatter(pce.get_var('N_T').to_np_array(), '%d') # Get position calibration flag pos_flag = pce.get_var('PD_FLAG') # If data is streaming, send data, otherwise send 'nil'. if streaming: # Create single length string to transmit. # Send calibrated arm positions if required. if pce.get_var('SEND_PD') == 1: # Get calibrated arm positions and rotations pos = stringFormatter(pce.get_var('POS').to_np_array().flatten(),'%.1f') rot = stringFormatter(pce.get_var('ROT').to_np_array().flatten(),'%d') sendString = "C_OUT=" + str(class_out) + ",POS=" + pos + ",ROT=" + rot pce.set_var('SEND_PD',0) else: sendString = "C_OUT=" + str(class_out) + ",C_NEW=" + str(class_new) + ",C_PC=" + str(class_pc) + ",N_C=" + ntot + ",N_R=" + nreps + ",N_T=" + npats + ",P_FLAG=" + str(pos_flag) else: sendString = "nil" # Try and send message, if it fails print message. try: sock.sendto(sendString, (addr)) except: connection = False print("Error in message transmission")
def updateWgAndCg(wg_data, cg_data, classList): tmp_wg = pce.get_var('WG_DATA').to_np_array() tmp_cg = pce.get_var('CG_DATA').to_np_array() for idx, i in enumerate(classList): tmp_wg[:, classList[idx]] = wg_data[:, idx] tmp_cg[0, classList[idx]] = cg_data[0, idx] pce.set_var('WG_DATA', tmp_wg) pce.set_var('CG_DATA', tmp_cg)
def updateWgAndCg(wg_adapt, cg_adapt, classList): tmp_wg = pce.get_var('WG_ADAPT').to_np_array() tmp_cg = pce.get_var('CG_ADAPT').to_np_array() for idx, i in enumerate(classList): tmp_wg[:, classList[idx]] = wg_adapt[:, idx] tmp_cg[0, classList[idx]] = cg_adapt[0, idx] pce.set_var('WG_ADAPT', tmp_wg) pce.set_var('CG_ADAPT', tmp_cg)
def updateMVC(mode): collect = 1 # N_MVC: Total number of windows used MVC_T = pce.get_var('MVC_T').to_np_array() # Update number of samples MVC_T[0, mode] += 1 # Set MVC samples pce.set_var('MVC_T', MVC_T) # If collected max samples if MVC_T[0, mode] == (sampThres - 1): pce.set_var('CLASS_ACTIVE', 0) pce.set_var('MODE', -1) MVC_R = pce.get_var('MVC_R').to_np_array() MVC_R[0, mode] += 1 pce.set_var('MVC_R', MVC_R) collect = 0 return collect
def updateMVC(mode): collect = 1 # N_MVC: Total number of windows used N_MVC = pce.get_var('N_MVC').to_np_array() # Update number of samples N_MVC[0, mode] = N_MVC[0, mode] + 1 # Set MVC samples pce.set_var('N_MVC', N_MVC) # If collected max samples if N_MVC[0, mode] == (sampThres - 1): pce.set_var('CLASS_ACTIVE', 0) pce.set_var('MODE', -1) collect = 0 return collect
def run(): # Try/catch to see if data already exists. try: pce.get_var('TRAIN_FLAG') except: # Initialise all variables. initialiseVariables() # Don't do anything if PCE is training. if pce.get_var('TRAIN_STATUS') != 1: # Get global variables. # global adaptNMCounter #yt # Define local variables. flag = int(pce.get_var('TRAIN_FLAG')) dnt_on = int(pce.get_var('DNT_ON')) N_R = pce.get_var('N_R').to_np_array() class_est = 0 armflag = int(pce.get_var('ARM_FLAG')) # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # PROCESS DATA # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Get raw DAQ data for the . raw_DAQ = np.array(pce.get_var('DAQ_DATA').to_np_array()[0:numEMG, :], order='F') # Get converted DAQ data between +/- voltRange. raw_conv = (raw_DAQ.astype(float) / (np.power(2, 16) - 1)) * (voltRange * 2) - voltRange # Get channel MAV. if CAPSMAV: chan_mav = pce.get_var('CHAN_MAV').to_np_array()[0:numEMG] else: # Get the absolute value of the data window. raw_abs = np.abs(raw_conv) # Get the average of the window. chan_mav = np.transpose([np.average(raw_abs, axis=1)]) # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # CLASSIFICATION # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Extract features from raw data feat_data = feat.extract(featVal, raw_DAQ) if pce.get_var('SAVE') == 1: dir_full = pce.get_var('DAQ_OUT_FNAME') dir = dir_full.split('/') daqname = dir[-1] dir = daqname.split('.') name = dir[0] ind = dir_full.rfind('/') ind = dir_full[0:ind].rfind('/') print(dir_full[0:ind]) saveWeights(dir_full[0:ind], name) pce.set_var('SAVE', 0) # IF FLAG == RESET if (flag == 999): print('RESET') # All variables will be initialised back to 0 (or their initial values). initialiseVariables() # IF FLAG == INITIAL CLASS TRAINER ACTIVATED elif (pce.get_var('CLASS_ACTIVE') == 0) & ((flag >= 0) & (flag < 99)): N_T = pce.get_var('N_T').to_np_array() # Reset the temp training counter for the specific class. N_T[0, flag] = 0 pce.set_var('N_T', N_T) # Toggle the class_active variable to 1. pce.set_var('CLASS_ACTIVE', 1) # IF FLAG == NO MOVEMENT elif (flag == 0): print 'here' if (armflag != 0): print('COLLECTING ' + classmap[flag]) # Prepare data for the 'no movement' class. Create means and covariance matricies. classPreparer(flag, feat_data, chan_mav, 'THRESH_VAL', 0, dnt_on) else: pce.set_var('COLLECTING', 0) # IF FLAG == ANY OTHER CLASS elif (99 > flag >= 1) & (N_R[0, 0] >= 1): # Compare the current channel MAV against the no movement threshold, if it exceeds then continue. if (armflag != 0) & (np.average(chan_mav) > (thresX * pce.get_var('THRESH_VAL'))): print('COLLECTING ' + classmap[flag]) # Prepare data for any movement other than 'no movement'. Create means and covariance matricies. classPreparer(flag, feat_data, chan_mav, ('CLASS_MAV' + str(flag)), 0, dnt_on) else: pce.set_var('COLLECTING', 0) # CLASSIFY AND FORWARD PASS # To classify the flag must be 0, 'no motion' data must be collected (N_R[0,0] >= 1). elif (flag == -1) & (N_R[0, 0] >= 1.0): out_map = pce.get_var('OUT_MAP').to_np_array() # Check that there is a new class to train. if pce.get_var('NEW_CLASS') == 1: print('TRAINING') # Create vector with just the values of classes trained (for remapping purposes). classList = np.nonzero(N_R)[1] # Update out_map. out_map[0, 0:len(classList)] = classList pce.set_var('OUT_MAP', out_map) # If channel data is poor, the LDA will fail to classify and will throw a singular matrix error. Catch this error. try: # Train using an LDA classifier. (wg_data, cg_data) = makeLDAClassifier(classList) # Add weights to WG and CG arrays and set to PCE. updateWgAndCg(wg_data, cg_data, classList) # Toggle new_class parameter. pce.set_var('NEW_CLASS', 0) except: print( 'ERROR: Bad pooled covariance data resulting in singular matrix.' ) # Get weights. Remove non-trained columns. wg_data = pce.get_var('WG_DATA').to_np_array()[:, out_map.tolist()[0]] cg_data = pce.get_var('CG_DATA').to_np_array()[0, out_map.tolist()[0]] # Forward pass to get estimate. lda_out = (np.dot(feat_data, wg_data) + cg_data) # Take argmax and remap for class value (i.e. 0 for 'no movement') class_est = float(out_map[0, (np.argmax(lda_out))]) # Set estimate to PCE. pce.set_var('CLASS_EST', class_est) # Print message with class estimation print('FORWARD - ' + str(class_est)) # DO NOTHING # In the event that no classes have been trained, print a message to the PCE log. # This statement is purely for debugging/logging purposes. It can be removed if necessary. else: print('NO ACTION') # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # PROPORTIONAL CONTROL # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ if useEnhanced: # Enhanced Technique. # Get total number of windows per class. N_C = pce.get_var('N_C').to_np_array() # Get summation of class/channel MAV. s_control = pce.get_var('S_CONTROL').to_np_array() # Determine average. s_controlAvg = s_control * (1 / N_C) # Calculate C from the equation. c_control = np.sum(np.square(s_controlAvg), axis=0) # Calculate proportional control. X = np.square( (1 / c_control) * (np.sum(s_controlAvg * chan_mav, axis=0))) else: # Incumbent Technique. X = np.ones((1, numModes)) * np.average(chan_mav) # Pre-filled matrix may contain NaN's and Inf's. If they exist make them zeros. for i in range(0, X.shape[0]): if (np.isnan(X[i]) or np.isinf(X[i])): X[i] = 0.0 # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # RAMPING # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ if rampEnabled and (rampTime != 0): # Step 1: bias each class numerator as not selected by subtracting 2 from numerator for i in range(0, numModes): ramp_numerator[:, i] -= 2 # Step 2: for any active DOF, reverse bias by adding 2, then add 1 more to decrease attenuation for that single class (total add = 3) for i in range(0, numModes): # Apply to active class as long as it's not 'no motion'. if ((i == class_est) and (class_est != 0)): # Increment ramp_numerator[:, i] += 3 # We never want to amplify, so make sure to cap the numerator. if (ramp_numerator[:, i] > ramp_denominator[:, i]): ramp_numerator[:, i] = ramp_denominator[:, i] # Scale signal X[i] = X[i] * ramp_numerator[:, i] / ramp_denominator[:, i] # Limit so that any non-active class numerator never falls below zero. if (ramp_numerator[:, i] < 0): ramp_numerator[:, i] = 0.0 # The original equation is 'O_j = B_j[G_j(X) - T_j]', where: # O_j is output speed, B_j is the boost, G_j is the gain, T_j is the threshold, and X is the channel MAV. # For PR the threshold and gain are typically 1, while the boost is 2. #prop_control = sig_boost * ((sig_gain * X) - ((voltRange / 100.0) * sig_thres)) prop_control = X # If the value of prop_control is below 0, it could be because of the threshold equation. If so, make the values 0. for i in range(0, prop_control.shape[0]): if (prop_control[i] < 0): prop_control[i] = 0.0 # Set proportional control value to PCE variable. pce.set_var('PROP_CONTROL', np.array(prop_control, dtype=float, order='F')) else: print('TRAINING HERE...')
def classPreparer(flag, feat_data, chan_mav, update_var, adapt, dnt): # Only build up weights if dnt is turned off. if dnt == 0: pce.set_var('COLLECTING', 1) # cov and mean are LDA variables. cov_C = pce.get_var('COV' + str(flag)).to_np_array() mean_C = pce.get_var('MN' + str(flag)).to_np_array() # N_C: Total number of windows used for training. This will increment to Inf. N_C = pce.get_var('N_C').to_np_array() # Get enhanced technique variables. s_control = pce.get_var('S_CONTROL').to_np_array() # Update the running average of training windows. update_val = updateAverage(pce.get_var(update_var), np.average(chan_mav), N_C[0, flag]) pce.set_var(update_var, update_val) # Update the cov and mean for LDA classification. (mean_C, cov_C, N_C[0, flag]) = updateMeanAndCov(mean_C, cov_C, N_C[0, flag], feat_data) # Determie enhanced proportional control. # Loop through all EMG channels. for i in range(0, numEMG): # Summate the current average EMG channel MAV with s_control. # Each class will have its own addition of EMG MAV windows. s_control[i, flag] += chan_mav[i, 0] # Update cov, mean, and total pce.set_var('COV' + str(flag), cov_C) pce.set_var('MN' + str(flag), mean_C) pce.set_var('N_C', N_C) # Update proportional control variables pce.set_var('S_CONTROL', s_control) # Only perform the following section if running regular training (not adaptation). if adapt == 0: # N_T: Number of windows used for training on the current repetition. N_T = pce.get_var('N_T').to_np_array() # Once the tmp training counter reaches the threshold, stop collecting data for training. if N_T[0, flag] == (samp_thres - 1): # Again, only update if dnt is turned off. if dnt == 0: # N_R: Number of training repetitions. N_R = pce.get_var('N_R').to_np_array() # Increment the repetition variable to indicate a new training session has been completed. N_R[0, flag] += 1 pce.set_var('N_R', N_R) # Set new_class to 1. This will indicate that a new training session is ready to be trained. pce.set_var('NEW_CLASS', 1) # Toggle the class_activate variable to 0. pce.set_var('CLASS_ACTIVE', 0) # Set the train_flag back to its standby value of -1. pce.set_var('TRAIN_FLAG', -1) pce.set_var('COLLECTING', 0) else: # Increment and set the temp training counter separately. N_T[0, flag] = N_T[0, flag] + 1 pce.set_var('N_T', N_T)
def updateThresh(cur_val): N_C = pce.get_var('N_C').to_np_array() thres_new = updateAverage(pce.get_var('THRESH_VAL'), cur_val, N_C[0, 0]) N_C[0, 0] = N_C[0, 0] + 1 pce.set_var('THRESH_VAL', thres_new) pce.set_var('N_C', N_C)
import pcepy.pce as pce import numpy as np # Number of modes/classes. numModes = 11 # Number of EMG channels. numEMG = int(len(pce.get_var('DAQ_CHAN').to_np_array()[0])) # Number of features. (10 for '47') featNum = 10 # Matrix size. matSize = numEMG * featNum print('RUNNING INITIALISE...') pce.set_var('TRAIN_FLAG', -1) pce.set_var('PD_FLAG', -1) pce.set_var('SEND_PD', 0) pce.set_var('CLASS_EST', -1) pce.set_var('THRESH_VAL', 0) pce.set_var('NEW_CLASS', 0) pce.set_var('CLASS_ACTIVE', 0) pce.set_var('ADAPT_ON', 0) pce.set_var('ADAPT_GT', -1) pce.set_var('DNT_ON', 0) pce.set_var('TARGET_DOF', 0) pce.set_var('TARGET_ARM', 0) pce.set_var('TRIAL_FLAG', 0) pce.set_var('ARM_FLAG', 0) pce.set_var('SAVE', 0) pce.set_var('COLLECTING', 0)
def run(): # Try/catch to see if data already exists. try: pce.get_var('MODE') except: # Initialise all variables. #initialiseVariables() print('no mode') # Don't do anything if PCE is training. if pce.get_var('TRAIN_STATUS') != 1: ctrl = int(pce.get_var('CTRL')) mode = int(pce.get_var('MODE')) collect = pce.get_var('COLLECT') N_T = pce.get_var('N_T').to_np_array() N_R = pce.get_var('N_R').to_np_array() chan_mav = pce.get_var('CHAN_MAV').to_np_array()[0:numEMG] cur_val = np.average(chan_mav) pce.set_var('CUR_VAL', cur_val) # IF MODE == RESET if mode == 999: # All variables will be initialised back to 0 (or their initial values). initialiseVariables() elif mode == 888: print 'ERROR' # IF MODE == START TEST/SAVE PARAMETERS elif mode == 500: datafolder = 'DATA/' + pce.get_var('DAQ_OUT_FNAME') datadir = os.path.abspath( os.path.join(os.path.dirname(__file__), '..', datafolder)) daqdir = datadir + '/DATA/DAQ' pvddir = datadir + '/DATA/PVD' if not os.path.exists(datadir): os.makedirs(daqdir) os.makedirs(pvddir) saveWeights(datadir) f = open(datadir + "/info.txt", "w+") f.write(pce.get_var('DAQ_OUT_FNAME')) f.write('\nctrl = {0:d}; noise = {1:d}; scale = {2:d}'.format( ctrl, int(pce.get_var('NOISE')), int(pce.get_var('NOISE_SCALE')))) f.close() pce.set_var('DAQ_OUT_FNAME', daqdir + '/data.DAQ') pce.set_var('PCE_VAR_OUT_FOLDER', pvddir) pce.set_var('MODE', -1) elif mode == 501: # Save weights saveWeights(datadir) # Update mode pce.set_var('MODE', -1) # UPDATE COLLECT FLAG if -5 < collect <= 0: collect -= 1 else: collect = 2 # Extract features from raw data feat_noisy = feat.extract( featVal, np.array(pce.get_var('DAQ_NOISY').to_np_array()[0:numEMG, :], dtype='uint16', order='F')) feat_raw = feat.extract( featVal, np.array(pce.get_var('DAQ_DATA').to_np_array()[0:numEMG, :], order='F')) pce.set_var('FEAT_RAW', feat_raw.astype(float, order='F')) pce.set_var('FEAT_NOISY', feat_noisy.astype(float, order='F')) if pce.get_var('NOISE') != 0: mvc = pce.get_var('MVC').to_np_array() # if not np.all(mvc): # pce.set_var('MODE', 888) # print 'COLLECT MVC!!' # else: feat_data = feat_noisy else: feat_data = feat_raw # IF CONTROL == PATTERN REC if ctrl == 1: print 'PATTERN REC' # IF MODE == INITIAL CLASS TRAINER ACTIVATED if (pce.get_var('CLASS_ACTIVE') == 0) & (0 <= mode < numClasses): # Reset the temp training counter for the specific class. N_T[0, mode] = 0 pce.set_var('N_T', N_T) # Toggle the class_active variable to 1. pce.set_var('CLASS_ACTIVE', 1) # IF MODE == TRAINING elif (mode == 0): print('COLLECTING ' + classmap[mode]) # Prepare data for the 'no movement' class. Create means and covariance matrices. collect = collectLDA(mode, cur_val, feat_data, 'THRESH_VAL') elif (1 <= mode < numClasses) & (N_R[0, 0] >= 1): # Compare the current channel MAV against the no movement threshold, if it exceeds then continue. if cur_val > (thresX * pce.get_var('THRESH_VAL')): print('COLLECTING ' + classmap[mode]) # Prepare data for any movement other than 'no movement'. Create means and covariance matricies. collect = collectLDA(mode, cur_val, feat_data, ('CLASS_MAV' + str(mode))) # CLASSIFY AND FORWARD PASS # To classify the mode must be 0, 'no motion' must be trained (N_R[0,0] >= 1), # and at least one other class needs to be trained (np.sum(N_R[1:numClasses]]) > 1). elif (mode == -1) & (N_R[0, 0] >= 1.0): out_map = pce.get_var('OUT_MAP').to_np_array() # Check that there is a new class to train. if pce.get_var('NEW_CLASS') == 1: print('TRAINING') # Create vector with just the values of classes trained (for remapping purposes). classList = np.nonzero(N_R)[1] # Update out_map. out_map[0, 0:len(classList)] = classList pce.set_var('OUT_MAP', out_map) # If channel data is poor, the LDA will fail to classify and will throw a singular matrix error. Catch this error. try: # Train using an LDA classifier. (wg_adapt, cg_adapt) = makeLDAClassifier(classList) # Add weights to WG and CG arrays and set to PCE. updateWgAndCg(wg_adapt, cg_adapt, classList) # Toggle new_class parameter. pce.set_var('NEW_CLASS', 0) except: print( 'ERROR: Bad pooled covariance data resulting in singular matrix.' ) # Get weights. Remove non-trained columns. wg_adapt = pce.get_var( 'WG_ADAPT').to_np_array()[:, out_map.tolist()[0]] cg_adapt = pce.get_var('CG_ADAPT').to_np_array()[ 0, out_map.tolist()[0]] # Add noise depending on testing phase. #if pce.get_var('NOISE') == 1: #feat_data = feat.extract(featVal, np.array(pce.get_var('DAQ_NOISY').to_np_array()[0:numEMG,:], dtype=np.uint16, order='F')) # Forward pass to get estimate. lda_out = (np.dot(feat_data, wg_adapt) + cg_adapt) # Take argmax and remap for class value (i.e. 0 for 'no movement') class_dec = float(out_map[0, (np.argmax(lda_out))]) # Set estimate to PCE. pce.set_var('CLASS_EST', class_dec) # Set velocity out. velocity = cur_val / ( 2 * pce.get_var('MID').to_np_array()[class_dec, 0]) if pce.get_var('OLD_CLASS') != class_dec: pce.set_var('RAMP', 1) pce.set_var('OLD_CLASS', class_dec) # Ramp proportional control ramp = pce.get_var('RAMP') velocity *= ramp / ramp_max pce.set_var('VEL', velocity) if ramp != ramp_max: pce.set_var('RAMP', ramp + 1) # Print message with class estimation print('FORWARD - ' + str(class_dec)) print('ramp - ' + str(ramp)) # IF CONTROL == REGRESSION elif ctrl == 2: print 'REGRESSION' if (pce.get_var('CLASS_ACTIVE') == 0) & (0 <= mode < numModels): # Reset the temp training counter for the specific class. N_T[0, mode] = 0 pce.set_var('N_T', N_T) # Toggle the class_active variable to 1. pce.set_var('CLASS_ACTIVE', 1) elif (0 <= mode < numModels): y_label = pce.get_var('Y_LABEL') print y_label if y_label != -1: print pce.get_var('THRESH_VAL') print("ylabel: ", y_label) print("COLLECTING TRAINING DATA") if y_label == 0: collect = collectLR(mode, feat_data, y_label) # Update threshold updateThresh(cur_val) elif cur_val > (thresX * pce.get_var('THRESH_VAL')): collect = collectLR(mode, feat_data, y_label) # TRAIN elif mode == -100: w = pce.get_var('W').to_np_array() for i in range(1, numModels): xx = pce.get_var('XX' + str(i)).to_np_array() xy = pce.get_var('XY' + str(i)).to_np_array() w[:, i] = linearModel(xx, xy) pce.set_var('W', w.astype(float, order='F')) # Set to prediction mode pce.set_var('MODE', -1) # PREDICT elif mode == -1: y_pred = pce.get_var('Y_EST').to_np_array() w = pce.get_var('W').to_np_array() for i in range(1, numModels): y_pred[i, 0] = predict(feat_data, w[:, i]) if y_pred[i, 0] <= 10 or y_pred[i, 0] > 100: y_pred[i, 0] = 0 print str(i) + ': ' + str(y_pred[i, 0]) pce.set_var('Y_EST', y_pred.astype(float, order='F')) # IF COLLECTING MVC elif ctrl == 3: print 'MVC' MVC_T = pce.get_var('MVC_T').to_np_array() if (pce.get_var('CLASS_ACTIVE') == 0) & (0 <= mode < numClasses): # Reset the temp training counter for the specific class. MVC_T[0, mode] = 0 pce.set_var('MVC_T', MVC_T) # Toggle the class_active variable to 1. pce.set_var('CLASS_ACTIVE', 1) else: # Collect resting threshold value if mode == 0: updateThresh(cur_val) collect = updateMVC(mode) elif (numClasses > mode > 0) & (cur_val > (1.5 * pce.get_var('THRESH_VAL'))): mvc = pce.get_var('MVC').to_np_array() MVC_R = pce.get_var('MVC_R').to_np_array() # Update mean MVC for current movement for i in range(0, numEMG): mvc[mode, i] = updateAverage( mvc[mode, i], chan_mav[i], sampThres * MVC_R[0, mode] + MVC_T[0, mode]) pce.set_var('MVC', mvc) collect = updateMVC(mode) # SET COLLECT FLAG print 'mode = ' + str(mode) + '; collect = ' + str(collect) pce.set_var('COLLECT', collect)
def collectLDA(mode, cur_val, feat_data, update_var): # indicate collecting training data collect = 1 # cov and mean are LDA variables. cov_C = pce.get_var('COV' + str(mode)).to_np_array() mean_C = pce.get_var('MN' + str(mode)).to_np_array() # N_C: Total number of windows used for training. This will increment to Inf. N_C = pce.get_var('N_C').to_np_array() # N_R: Number of training repetitions. N_R = pce.get_var('N_R').to_np_array() # N_T: Number of windows used for training on the current repetition. N_T = pce.get_var('N_T').to_np_array() # Update the running average of training windows. update_val = updateAverage(pce.get_var(update_var), cur_val, N_C[0, mode]) pce.set_var(update_var, update_val) # Update the cov and mean for LDA classification. (mean_C, cov_C, N_C[0, mode]) = updateMeanAndCov(mean_C, cov_C, N_C[0, mode], feat_data) # Update cov, mean, and total pce.set_var('COV' + str(mode), cov_C) pce.set_var('MN' + str(mode), mean_C) pce.set_var('N_C', N_C) # Increment and set the temp training counter separately. N_T[0, mode] = N_T[0, mode] + 1 pce.set_var('N_T', N_T) print(N_T[0, mode]) # Update mean MVC for current movement mid = pce.get_var('MID').to_np_array() mid[mode, 0] = updateAverage(mid[mode, 0], cur_val, N_T[0, mode]) pce.set_var('MID', mid) # Once the tmp training counter reaches the threshold, stop collecting data for training (As the variable starts at 0 we have to subtract 1 from sampThres). if N_T[0, mode] == (sampThres - 1): # Update collect flag collect = 0 # Increment the repetition variable to indicate a new training session has been completed. N_R[0, mode] += 1 pce.set_var('N_R', N_R) # Set new_class to 1. This will indicate that a new training session is ready to be trained. pce.set_var('NEW_CLASS', 1) # Toggle the class_activate variable to 0. pce.set_var('CLASS_ACTIVE', 0) # Set the mode back to its standby value of -1. pce.set_var('MODE', -1) return collect
def initialiseVariables(): print('RESET') # Common variables pce.set_var('CTRL', 1) pce.set_var('MODE', -1) pce.set_var('CUR_VAL', 0) pce.set_var('THRESH_VAL', 0) pce.set_var('NOISE', 0) pce.set_var('NOISE_CH', 0) pce.set_var('NOISE_SCALE', 0) pce.set_var('COLLECT', 2) pce.set_var('IN_TRIAL', 0) pce.set_var('MVC_R', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('MVC_T', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('MVC', np.zeros((numClasses, numEMG), dtype=float, order='F')) pce.set_var('N_C', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('N_R', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('N_T', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('DAQ_NOISY', np.zeros((numEMG, window), dtype=float, order='F')) pce.set_var('FEAT_RAW', np.zeros((1, matSize), dtype=float, order='F')) pce.set_var('FEAT_NOISY', np.zeros((1, matSize), dtype=float, order='F')) # Variables for PR only pce.set_var('NEW_CLASS', 0) pce.set_var('OLD_CLASS', 0) pce.set_var('VEL', 0) pce.set_var('RAMP', 1) pce.set_var('CLASS_ACTIVE', 0) pce.set_var('CLASS_EST', -1) pce.set_var('OUT_MAP', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('WG_ADAPT', np.zeros((matSize, numClasses), dtype=float, order='F')) pce.set_var('CG_ADAPT', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('MID', np.zeros((numClasses, 1), dtype=float, order='F')) for i in range(0, numClasses): pce.set_var('COV' + str(i), np.zeros((matSize, matSize), dtype=float, order='F')) pce.set_var('MN' + str(i), np.zeros((1, matSize), dtype=float, order='F')) pce.set_var('CLASS_MAV' + str(i), 0) # Variables for regression only pce.set_var('Y_LABEL', -1) pce.set_var('X', np.zeros((sampThres, matSize + 1), dtype=float, order='F')) pce.set_var('Y', np.zeros((sampThres, 1), dtype=float, order='F')) pce.set_var('Y_EST', np.zeros((numModels, 1), dtype=float, order='F')) pce.set_var('ZERO_FLAGS', np.zeros((numModels, numModels), dtype=float, order='F')) pce.set_var('W', np.zeros((matSize + 1, numModels), dtype=float, order='F')) for i in range(1, numModels): pce.set_var( 'XX' + str(i), np.zeros((matSize + 1, matSize + 1), dtype=float, order='F')) pce.set_var('XY' + str(i), np.zeros((matSize + 1, 1), dtype=float, order='F'))
####################################################################################################################### # Function : load_weights(args) # args : folder directory: (../DATA/foldername), controller type: 'LR' or 'PR' # Description : This function loads previously calculated weights for the LDA/LR controllers ####################################################################################################################### # Import all the required modules. These are helper functions that will allow us to get variables from CAPS PC import sys import pcepy.pce as pce import numpy as np if sys.argv[2] == 'LR': print 'Loading linear regression weights...' w = np.genfromtxt(str(sys.argv[1]) + 'w.csv', delimiter=',') pce.set_var('W', w.astype(float, order='F')) else: numClasses = 5 out_map = pce.get_var('OUT_MAP').to_np_array() print 'Loading LDA weights...' cg = pce.get_var('CG_ADAPT').to_np_array() wg = np.genfromtxt(str(sys.argv[1]) + 'wg.csv', delimiter=',') cg_temp = np.genfromtxt(str(sys.argv[1]) + 'cg.csv', delimiter=',') for i in range(0, numClasses): cg[0,i] = cg_temp[i] mid = np.genfromtxt(str(sys.argv[1]) + 'mid.csv', delimiter=',') NR = 3 * np.ones((1, numClasses), dtype=float, order='F') pce.set_var('N_R', NR) # Create vector with just the values of classes trained (for remapping purposes). classList = np.nonzero(NR)[1] # Update out_map. out_map[0,0:len(classList)] = classList
def receiveData(): # Get global variables. global streaming # Get data from embedded system. data = GrabData(sock) # HEARTBEAT # Message pushed frequently to indicate the connection is still live. if data == "HeartBeat": print("HeartBeat") # DISCONNECT # If the message 'Disconnect' is sent, then close connection. elif data == "Disconnect": sock.close() print("Connection Closed") # CAN # If the message contains 'can.send' a CAN message is being sent to be pushed to the embedded system. elif "can|send" in data: print("Sending CAN message") # NID is the first hex number of the can.send string nid = data.split('|')[2] # Set NID, priority, and mode. canmessenger.set_nid(int(nid, 16)) canmessenger.set_priority(0x00) canmessenger.set_mode(0x01) # Bits in string format bits_str = data.split('|')[3:10] # Get CAN data into a presentable form. candata = np.zeros((5, ), dtype=np.int) # Put bits in candata, converted from string to int for iDigit in range(0, len(bits_str)): candata[iDigit] = int(bits_str[iDigit], 16) try: # Attempt to send data. canmessenger.set_data(candata[0], candata[1], candata[2], candata[3], candata[4]) canmessenger.send() # Succeeded in sending message. Print to screen and send success handshake. print(" CAN send value sent: " + candata) sock.sendto('ACKCON=1:' + data, (addr)) except: # Failed to send message. Print to screen and send failed handshake. print(" Error: CAN send value not sent") sock.sendto('ACKCON=0:' + data, (addr)) pass # PCE # If the message contains 'pce.set', a PCE message is being sent to be set on the embedded system. elif "pce|set" in data: print("Sending PCE message") # Bits in string format bits_str = data.split('|') # If the data contains 'cmd', then this is a command instruction. if "cmd" in data: try: # Attempt to sent the message to the embedded system. pce.send_cmd(int(bits_str[3])) # If the command was 3 (start streaming), then set bool to true. if int(bits_str[3]) == 3: streaming = True # If the command was 4 (stop streaming), then set bool to false. if int(bits_str[3]) == 4: streaming = False # Succeeded in sending message. Print to screen and send success handshake. print(" PCE cmd value sent: " + bits_str[3]) sock.sendto('ACKCON=1:' + data, (addr)) except: # Failed to send message. Print to screen and send failed handshake. print(" Error: PCE cmd value not sent") sock.sendto('ACKCON=0:' + data, (addr)) pass # If the data contains 'var', then this is a variable on the embedded system to be updated. elif "var" in data: try: # todo: improve this method to determine if a string or int is to be sent. if "DAQ_OUT_FNAME" in bits_str[3]: pce.set_var(bits_str[3], bits_str[4]) else: pce.set_var(bits_str[3], int(bits_str[4])) # Succeeded in sending message. Print to screen and send success handshake. print(" PCE var value sent: " + bits_str[4]) sock.sendto('ACKCON=1:' + data, (addr)) except: # Failed to send message. Print to screen and send failed handshake. print(" Error: PCE var value not sent") sock.sendto('ACKCON=0:' + data, (addr)) pass # Update log # If the message contains 'log.set', a new line of progress is written to a log file. elif "log|set" in data: print("Updating log file") try: # Break data at the full stops. splitString = data.split('|') # The third block contains the location, the fourth the data. logLoc = splitString[2] + ".txt" logDat = splitString[3] # Write data to file ('a' for 'append', '+' creates the file if it doesn't exist). with open(logLoc, 'a+') as f: f.write(logDat + '\n') # Succeeded in sending message. Print to screen and send success handshake. print(" Log updated") sock.sendto('ACKCON=1:' + data, (addr)) except: # Failed to send message. Print to screen and send failed handshake. print(" Error: Log not updated") sock.sendto('ACKCON=0:' + data, (addr)) pass
def collectLR(mode, data, y_out): # Set collecting data flag to 1 collect = 1 # Get number of samples, x, and y variables N_T = pce.get_var('N_T').to_np_array() x = pce.get_var('X').to_np_array() y = pce.get_var('Y').to_np_array() # Add bias ones to x data temp = np.c_[data, 1] # Save x and y data x[N_T[0, mode], :] = temp y[N_T[0, mode], :] = y_out pce.set_var('X', x.astype(float, order='F')) pce.set_var('Y', y.astype(float, order='F')) # Increment the number of x points N_T[0, mode] += 1 if N_T[0, mode] == (sampThres - 1): # set collecting data flag to 0 collect = 0 # Increment the repetition variable to indicate a new training session has been completed. N_R = pce.get_var('N_R').to_np_array() N_R[0, mode] += 1 pce.set_var('N_R', N_R) # Set the mode back to its standby value of -1. pce.set_var('MODE', -1) # Set the y_label back to its standby value of -1. pce.set_var('Y_LABEL', -1) # Toggle the class_activate variable to 0. pce.set_var('CLASS_ACTIVE', 0) # Zero y matrix for all other models y_zeros = np.zeros((sampThres, 1), dtype=float, order='F') # Randomly select samples ind = np.random.choice(sampThres - 1, sampThres / (2 * 6), replace=False) # Update X'X and X'Y matrices for i in range(1, numModels): xx = pce.get_var('XX' + str(i)).to_np_array() xy = pce.get_var('XY' + str(i)).to_np_array() if i == mode: #if np.any(y): (xx, xy) = updateMat(xx, xy, x, y) #else: #(xx, xy) = updateMat(xx, xy, x[0:sampThres-1:2,:], y[0:sampThres-1:2,:]) else: #if np.any(y): (xx, xy) = updateMat(xx, xy, x, y_zeros) #(xx, xy) = updateMat(xx, xy, x[ind,:], y_zeros[ind,:]) pce.set_var('XX' + str(i), xx.astype(float, order='F')) pce.set_var('XY' + str(i), xy.astype(float, order='F')) pce.set_var('N_T', N_T) return collect
numModels = 5 # Number of EMG channels. numEMG = 6 # Number of features. (10 for '47') featNum = 4 # Matrix size. matSize = numEMG * featNum # Sample threshold sampThres = 100 # Number of samples in one frame window = 200 print('RUNNING INITIALISE...') # Common variables pce.set_var('CTRL',1) pce.set_var('MODE', -1) pce.set_var('CUR_VAL', 0) pce.set_var('THRESH_VAL', 0) pce.set_var('NOISE', 0) pce.set_var('NOISE_CH', 0) pce.set_var('NOISE_SCALE', 0) pce.set_var('COLLECT', 2) pce.set_var('IN_TRIAL', 0) pce.set_var('MVC_R', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('MVC_T', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('MVC', np.zeros((numClasses, numEMG), dtype=float, order='F')) pce.set_var('N_C', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('N_R', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('N_T', np.zeros((1, numClasses), dtype=float, order='F')) pce.set_var('DAQ_NOISY', np.zeros((numEMG, window), dtype=float, order='F'))
def run(): t = pce.get_var('THRESH_VAL') print t t += 1 pce.set_var('THRESH_VAL', t)