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 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_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 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 ArraySizes():
# Determine and format the number of classes (based off size of N_T) and channel size (based of CHAN_MAV)
class_SIZE = str(pce.get_var('N_T').to_np_array().shape[1])
chan_SIZE = str(len(pce.get_var('CHAN_MAV').to_np_array()))
# Format array sizes into concatenated string.
arraySize = ";" + class_SIZE + "," + chan_SIZE
# Return string.
return arraySize
def saveWeights(dir, name):
np.savetxt(dir + '/CSV/' + name + '_wg.csv',
pce.get_var('WG_DATA').to_np_array(),
fmt='%.20f',
delimiter=',')
np.savetxt(dir + '/CSV/' + name + '_cg.csv',
pce.get_var('CG_DATA').to_np_array(),
fmt='%.20f',
delimiter=',')
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 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 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 saveWeights(datadir):
np.savetxt(datadir + '/' + 'mid.csv',
pce.get_var('MID').to_np_array(),
fmt='%.20f',
delimiter=',')
np.savetxt(datadir + '/' + 'mvc.csv',
pce.get_var('MVC').to_np_array(),
fmt='%.20f',
delimiter=',')
np.savetxt(datadir + '/' + 'w.csv',
pce.get_var('W').to_np_array(),
fmt='%.20f',
delimiter=',')
np.savetxt(datadir + '/' + 'wg.csv',
pce.get_var('WG_ADAPT').to_np_array(),
fmt='%.20f',
delimiter=',')
np.savetxt(datadir + '/' + 'cg.csv',
pce.get_var('CG_ADAPT').to_np_array(),
fmt='%.20f',
delimiter=',')
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
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 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 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)
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...')
# 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
pce.set_var('OUT_MAP', out_map)
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 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 "NAME" in bits_str[3]:
pce.set_var(bits_str[3], bits_str[4])
elif ("POS" in bits_str[3]) or ("ROT" in bits_str[3]):
current = pce.get_var(bits_str[3]).to_np_array()
for i in range(3):
current[int(bits_str[4]),i] = float(bits_str[i+5])
pce.set_var(bits_str[3], current)
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 sendData():
# Get class prediction (CLASS_EST) and whether a new class has just been trained (NEW_CLASS).
class_out = pce.get_var('CLASS_EST')
collect = pce.get_var('COLLECT')
cur_val = pce.get_var('CUR_VAL')
velocity = pce.get_var('VEL')
# Get all class MAV, format into comma deliminated string. Shape[1] of N_T is used to get the number of modes (typically 7).
#y_est = ';'.join(['%.5f' % num2 for num2 in [pce.get_var('Y_EST' + str(num)) for num in range(0, numModels)]])
y_est = stringFormatter(pce.get_var('Y_EST').to_np_array(), '%.5f')
# Get all EMG channels MAV, format into comma deliminated string.
chan_mav = ';'.join(
['%.5f' % num for num in pce.get_var('CHAN_MAV').to_np_array()])
# Get the number of patterns (N_T) and number of repetition (N_R).
npats = stringFormatter(pce.get_var('N_T').to_np_array(), '%d')
nreps = stringFormatter(pce.get_var('N_R').to_np_array(), '%d')
mvcpats = stringFormatter(pce.get_var('MVC_T').to_np_array(), '%d')
mvcreps = stringFormatter(pce.get_var('MVC_R').to_np_array(), '%d')
# Get the control type
ctrl = pce.get_var('CTRL')
# If data is streaming, send data, otherwise send 'nil'.
if streaming:
# Create single length string to transmit.
if ctrl == 1:
sendString = "C_OUT=" + str(class_out) + ",COLL=" + str(
collect) + ",VEL=" + str(
velocity) + ",N_T=" + npats + ",N_R=" + nreps + ",STR"
elif ctrl == 2:
sendString = "Y_EST=" + y_est + ",COLL=" + str(
collect) + ",N_T=" + npats + ",N_R=" + nreps + ",STR"
else:
sendString = "COLL=" + str(
collect) + ",N_T=" + mvcpats + ",N_R=" + mvcreps + ",STR"
else:
sendString = "nil"
print sendString
# Try and send message, if it fails print message.
try:
sock.sendto(sendString, (addr))
except:
connection = False
print("Error in message transmission")
4: 'WRIST SUP.',
5: 'WRIST FLEX.',
6: 'WRIST EXT.',
7: 'WRIST ADD.',
8: 'WRIST ABD.',
9: 'ELBOW FLEX.',
10: 'ELBOW EXT.'
}
# Specify where the saved data is stored.
datafolder = 'DATA'
datadir = os.path.abspath(
os.path.join(os.path.dirname(__file__), '..', datafolder))
# Number of modes/classes.
numModes = int(len(classmap))
# Number of EMG channels.
numEMG = int(len(pce.get_var('DAQ_CHAN').to_np_array()[0]))
# Feature value ('47'; time domain and autoregression)
featVal = 47
# Number of features. (10 for '47')
featNum = 10
# Matrix size.
matSize = numEMG * featNum
# Threshold multiplier
thresX = 1.1
# Sample threshold
samp_thres = 100
# Voltage range of EMG signal (typically +/- 5V)
voltRange = 5
# Signal boost, gain, and threshold variables (threshold between 1 and 100)
sig_boost = 2
sig_gain = 1
# CAPS can find them. You should only need to run this once. If you're seeing the error often, contact CBM.
# 1: Open PuTTY and navigate to the mode's SLAVE FOLDER:
# a) cd /
# b) cd /config/mode/caps/*MODENAME*/SLAVE/
# 2: Type the following:
# a) python initialise.py
# 3: A message should state that initialise is running, and then a second message stating that it has completed.
# 4: Close PuTTY and power cycle the device. Null variables will now exist in the system and CAPS should operate normally.
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)
def run():
t = pce.get_var('THRESH_VAL')
print t
t += 1
pce.set_var('THRESH_VAL', t)
