def test_already_closed():
dev = MpDevice(Dummy())
dev.start()
res = dev.read()
dev.stop()
with raises(RuntimeError):
dev.read()
def test_multi_devs():
dev1 = MpDevice(Dummy())
dev2 = MpDevice(Dummy())
with dev1, dev2:
sleep(0.2)
time1, data1 = dev1.read()
time2, data2 = dev2.read()
print(time1[-5:], time2[-5:])
def test_views():
dev = MpDevice(Dummy(), use_views=True)
with dev:
datas = []
sleep(0.1)
time, data = dev.read()
datas.append(data)
sleep(0.1)
time, data = dev.read()
datas.append(data)
print(datas)
assert (datas[0][2, 3] == datas[1][2, 3])
def test_restart():
dev = MpDevice(Dummy())
with dev:
sleep(0.2)
res = dev.read()
with dev:
sleep(0.2)
res2 = dev.read()
print(res[0])
print(res2[0])
assert (res[0].any() and res2[0].any())
assert (res[0][-1] < res2[0][0])
def test_list():
dev = MpDevice(DummyList())
with dev:
sleep(0.2)
time, data = dev.read()
assert (data.shape[0] > 10)
assert (data.shape[0] == time.shape[0])
def test_none():
dev = MpDevice(NoData())
with dev:
sleep(0.2)
res = dev.read()
assert (res is None)
def test_struct():
dev = MpDevice(StructObs())
with dev:
sleep(0.2)
time, data = dev.read()
print(data)
assert (data.shape[0] > 10)
assert (data.shape[0] == time.shape[0])
def test_nones():
dev = MpDevice(SometimesNot())
with dev:
sleep(0.2)
time, data = dev.read()
print(data.shape)
assert (data.shape[0] > 10)
assert (data.shape[0] == time.shape[0])
def test_mono():
dev = MpDevice(Incrementing(), buffer_len=10)
with dev:
sleep(1)
time, val = dev.read()
print(time, val)
assert (all(np.diff(time) > 0))
assert (all(np.diff(val) > 0))
def test_remote_clock():
dev = MpDevice(Dummy(clock=mono_clock.get_time))
sleep(0.5)
with dev:
sleep(0.1)
time, data = dev.read()
tmp = mono_clock.get_time()
print(tmp - time[-1])
assert (tmp - time[-1] == approx(1.0 / Dummy.sampling_frequency, abs=3e-3))
def test_ringbuffer():
dev = MpDevice(Dummy(), buffer_len=1)
with dev:
sleep(1)
time, val = dev.read()
assert (time.shape[0] == 1)
assert (val.shape == (1, 5))
print(time, val)
def test_device_single():
dev = MpDevice(Dummy())
dev.start()
sleep(0.25)
res = dev.read()
dev.stop()
assert (res is not None)
time, data = res
print(np.diff(time))
print(data.shape)
assert (data.shape[0] > 10)
assert (data.shape[0] == time.shape[0])
def test_freq():
original_fs = Dummy.sampling_frequency
Dummy.sampling_frequency = 1
dev = MpDevice(Dummy())
with dev:
sleep(4)
time, val = dev.read()
assert (time.shape[0] == 1)
assert (val.shape == (1, 5))
print(time, val)
Dummy.sampling_frequency = original_fs
def test_read_multi():
dev = MpDevice(Incrementing())
with dev:
times, dats = [], []
for i in range(500):
data = dev.read()
if data:
times.append(data.time)
dats.append(data.data)
sleep(0.016)
times = np.hstack(times)
vals = np.hstack(dats)
print(times)
print(vals)
assert (all(np.diff(times)) > 0)
assert (all(np.diff(vals)) > 0)
class LivePlot(pg.GraphicsLayoutWidget):
def __init__(self, device):
super(LivePlot, self).__init__()
self.device = MpDevice(device, use_views=True)
self.current_data = None
self.lines = []
dummy = np.ones(4)
self.fig = self.addPlot(title='Foobar')
self.fig.showGrid(x=True, y=True, alpha=0.4)
self.fig.setClipToView(True)
mi, ma = 0, 4000
self.fig.setRange(yRange=[mi, ma])
self.fig.setLimits(yMin=mi, yMax=ma)
for j in range(3):
pen = pg.mkPen(color=pg.intColor(j, hues=3, alpha=220), width=1)
self.lines.append(self.fig.plot(dummy, dummy, pen=pen))
self.timer = QtCore.QTimer()
self.timer.timeout.connect(self.update)
self.timer.start(0)
self.playing = True
def update(self):
pos = self.device.read()
if pos is None:
return
if self.current_data is None:
self.current_data = pos.data
self.current_time = pos.time
elif self.current_data.shape[0] < self.device.device.sampling_frequency:
self.current_data = np.hstack((self.current_data, pos.data))
self.current_time = np.hstack((self.current_time, pos.time))
else:
ps0 = -pos.data.shape[0]
self.current_data = np.roll(self.current_data, ps0, axis=0)
self.current_data[ps0:] = pos.data
self.current_time = np.roll(self.current_time, ps0)
self.current_time[ps0:] = pos.time
if self.playing:
self.lines[0].setData(y=self.current_data[:]['digital'] * 2000)
self.lines[1].setData(y=self.current_data[:]['a1'])
self.lines[2].setData(y=self.current_data[:]['a2'])
print(pos.data[-1])
if __name__ == '__main__':
from toon.input import MpDevice
from toon_rawinput.keyboard import Keyboard
from toon_rawinput.mouse import Mouse
print('sleeping-- plug in test device.')
sleep(5)
print('done sleeping-- good luck')
mydev = MpDevice(Keyboard(clock=ptb.GetSecs))
mytimes = []
total_time = 60 * 2
with mydev:
t0 = time()
while time() - t0 < total_time:
res = mydev.read()
if res:
#print('mine: %s' % res.time)
mytimes.append(res.time)
sleep(0.02)
import matplotlib.pyplot as plt
import numpy as np
mytimes = np.hstack(mytimes)
mylen = len(mytimes)
if mylen % 2 != 0:
mytimes = mytimes[1:]
mydff = np.diff(mytimes[::2])
plt.plot(mydff)
class PinchTask(ShowBase, GripStateMachine):
DATA_DIR = 'data'
def __init__(self, id, session, hand, block, mode, wrist):
ShowBase.__init__(self)
GripStateMachine.__init__(self)
base.disableMouse()
wp = WindowProperties()
wp.setSize(1920,1080)
wp.setFullscreen(True)
base.win.requestProperties(wp)
self.sub_id = str(id)
self.sess_id = str(session)
self.hand = str(hand)
self.block = str(block)
self.mode = str(mode)
self.wrist = str(wrist)
self.prev_blk = os.path.join(self.DATA_DIR,'exp_1',self.sub_id,self.sess_id,self.wrist,self.hand,"B0")
if self.wrist == 'pron':
self.table = np.loadtxt('src/pinch_task/trialtable_pron.csv',dtype='str',delimiter=',',skiprows=1)
elif self.wrist == 'flex':
self.table = np.loadtxt('src/pinch_task/trialtable_flex.csv',dtype='str',delimiter=',',skiprows=1)
else:
raise NameError('Wrist position not found')
try:
self.prev_table = np.loadtxt(os.path.join(self.prev_blk, 'final_targets.csv'),dtype='str',delimiter=',',skiprows=1)
indices = {}
for i in range(self.prev_table.shape[0]):
indices[self.prev_table[i,1]] = int(self.prev_table[i,0])-1
for i in range(self.table.shape[0]):
self.table[i,11] = self.prev_table[indices[self.table[i,1].strip()],11]
self.table[i,12] = self.prev_table[indices[self.table[i,1].strip()],12]
self.table[i,13] = self.prev_table[indices[self.table[i,1].strip()],13]
except:
print('Previous target file not found')
self.table = np.array([[item.strip() for item in s] for s in self.table])
##################################################
#only use rows relevant to this block
#HARDCODED! NOTE IN LOG SHEET
spec_table = []
for i in range(self.table.shape[0]):
if int(self.block)%4 == 0:
if "(p)" in self.table[i,2]:
spec_table.append(self.table[i])
elif int(self.block)%4 == 1:
if "(t)" in self.table[i,2]:
spec_table.append(self.table[i])
elif int(self.block)%4 == 2:
if "(s)" in self.table[i,2]:
spec_table.append(self.table[i])
elif int(self.block)%4 == 3:
if "(t+s)" in self.table[i,2]:
spec_table.append(self.table[i])
###################################################
self.table = np.array(spec_table)
self.session_dir = os.path.join(self.DATA_DIR,'exp_1',self.sub_id,self.sess_id,self.wrist,self.hand)
self.subjinfo = self.sub_id + '_' + self.sess_id + '_' + self.hand + '_log.yml'
self.p_x,self.p_y,self.p_a = GET_POS(self.session_dir,self.subjinfo,self.hand,self.wrist)
self.rotmat = ROT_MAT(self.p_a,self.hand)
self.setup_text()
self.setup_lights()
self.setup_camera()
self.trial_counter = 0
self.load_models()
self.load_audio()
self.update_trial_command()
self.countdown_timer = CountdownTimer()
self.hold_timer = CountdownTimer()
self.cTrav = CollisionTraverser()
self.chandler = CollisionHandlerEvent()
self.chandler.addInPattern('%fn-into-%in')
self.chandler.addAgainPattern('%fn-again-%in')
self.chandler.addOutPattern('%fn-outof-%in')
self.attachcollnodes()
taskMgr.add(self.read_data, 'read')
for i in range(5):
taskMgr.add(self.move_player, 'move%d' % i, extraArgs = [i], appendTask=True)
taskMgr.add(self.log_data, 'log data')
taskMgr.add(self.update_state, 'update_state', sort=1)
self.accept('space', self.space_on)
self.accept('escape', self.clean_up)
self.space = False
self.statenum = list()
self.max_time = 20
self.med_data = None
self.grip_dir = os.path.join(self.DATA_DIR,'exp_1',self.sub_id,self.sess_id,self.wrist,self.hand,"B"+self.block)
if not os.path.exists(self.grip_dir):
print('Making new folders: ' + self.grip_dir)
os.makedirs(self.grip_dir)
self.dev = MpDevice(RightHand(calibration_files=['calibs/cal_mat_15.mat', # thumb
'calibs/cal_mat_31.mat',
'calibs/cal_mat_8.mat',
'calibs/cal_mat_21.mat',
'calibs/cal_mat_13.mat'], clock=mono_clock.get_time))
self.init_ser()
############
#SET UP HUD#
############
def setup_text(self):
#OnscreenImage(parent=self.cam2dp, image='models/background.jpg')
self.bgtext = OnscreenText(text='Not recording.', pos=(-0.8, 0.8),
scale=0.08, fg=(0, 0, 0, 1),
bg=(1, 1, 1, 1), frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.bgtext.reparentTo(self.aspect2d)
self.dirtext = OnscreenText( pos=(-0.6, 0.65),
scale=0.08, fg=(0, 0, 0, 1),
bg=(1, 1, 1, 1), frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.dirtext.reparentTo(self.aspect2d)
##########################
#SET UP SCENE AND PLAYERS#
##########################
def setup_lights(self):
pl = PointLight('pl')
pl.setColor((1, 1, 1, 1))
plNP = self.render.attachNewNode(pl)
plNP.setPos(-10, -10, 10)
self.render.setLight(plNP)
pos = [[[0, 0, 50], [0, 0, -10]],
[[0, -50, 0], [0, 10, 0]],
[[-50, 0, 0], [10, 0, 0]]]
for i in pos:
dl = Spotlight('dl')
dl.setColor((1, 1, 1, 1))
dlNP = self.render.attachNewNode(dl)
dlNP.setPos(*i[0])
dlNP.lookAt(*i[1])
dlNP.node().setShadowCaster(False)
self.render.setLight(dlNP)
def setup_camera(self):
self.cam.setPos(0, -4, 12)
self.cam.lookAt(0, 2, 0)
self.camLens.setFov(90)
def load_models(self):
self.back_model = self.loader.loadModel('models/back')
self.back_model.setScale(10, 10, 10)
if self.hand == "Left":
self.back_model.setH(90)
self.back_model.reparentTo(self.render)
self.player_offsets = [[self.p_x[0]-5, self.p_y[0]+3, 0], [self.p_x[1]-2.5, self.p_y[1]+4.5, 0], [self.p_x[2], self.p_y[2]+5, 0],
[self.p_x[3]+2.5, self.p_y[3]+4.5, 0], [self.p_x[4]+5, self.p_y[4]+3, 0]]
self.p_col =[[0,0,250],[50,0,200],[125,0,125],[200,0,50],[250,0,0]]
if self.hand == 'Left':
self.p_col = self.p_col[::-1]
self.players = list()
self.contacts = list()
for counter, value in enumerate(self.player_offsets):
self.players.append(self.loader.loadModel('models/target'))
self.contacts.append(False)
self.players[counter].setPos(*value)
self.players[counter].setScale(0.2, 0.2, 0.2)
self.players[counter].setColorScale(
self.p_col[counter][0]/255, self.p_col[counter][1]/255, self.p_col[counter][2]/255, 1)
self.players[counter].reparentTo(self.render)
self.players[counter].show()
self.target_select()
def load_audio(self):
self.pop = self.loader.loadSfx('audio/Blop.wav')
self.buzz = self.loader.loadSfx('audio/Buzzer.wav')
############################
#SET UP COLLISION MECHANICS#
############################
def attachcollnodes(self):
for i in range(5):
self.fromObject = self.players[i].attachNewNode(CollisionNode('colfromNode'+str(i)))
self.fromObject.node().addSolid(CollisionSphere(0,0,0,1))
self.cTrav.addCollider(self.fromObject, self.chandler)
for i in range(5):
self.accept('colfromNode%d-into-colintoNode' % i, self.collide1,[i])
self.accept('colfromNode%d-again-colintoNode' % i, self.collide2,[i])
self.accept('colfromNode%d-outof-colintoNode' % i, self.collide3,[i])
def collide1(self,f,collEntry):
if f in self.highlighted_indices:
self.players[f].setColorScale(0,1,0,0)
self.tar.setColorScale(0.2,0.2,0.2,1)
self.tar.setAlphaScale(0.7)
self.contacts[f] = True
taskMgr.doMethodLater(self.delay,self.too_long,'too_long%d' % f,extraArgs = [f])
self.sendsig(1,f,100)
def collide2(self,f,collEntry):
if f in self.highlighted_indices:
dist = np.sqrt(self.distances[f][1]^2+self.distances[f][2]^2+self.distances[f][3]^2)
self.sendsig(2,f,int(10/dist)+100)
for i in self.highlighted_indices:
if self.contacts[i] == False:
return
taskMgr.remove('too_long%d' % f)
def collide3(self,f,collEntry):
taskMgr.remove('too_long%d' % f)
self.reset_fing(f)
self.tar.setColorScale(0.1,0.1,0.1,1)
self.tar.setAlphaScale(0.7)
def too_long(self,f):
self.reset_fing(f)
self.tar.setColorScale(0.5,0.2,0.2,1)
self.tar.setAlphaScale(0.7)
def reset_fing(self,f):
self.players[f].setColorScale(
self.p_col[f][0]/255, self.p_col[f][1]/255, self.p_col[f][2]/255, 1)
self.contacts[f] = False
self.sendsig(3,f,0)
###############
#TARGET THINGS#
###############
def show_target(self):
self.target_select()
self.intoObject = self.tar.attachNewNode(CollisionNode('colintoNode'))
if self.table[self.trial_counter,7] == "sphere":
self.intoObject.node().addSolid(CollisionSphere(0,0,0,1))
elif self.table[self.trial_counter,7] == "cylinder":
self.intoObject.node().addSolid(CollisionTube(0,0,-2,0,0,2,1))
else:
raise NameError("No such collision type")
self.tar.show()
#hide players not related to target
for i in range(5):
if i not in self.highlighted_indices:
self.players[i].hide()
def target_select(self):
self.tgtscx=float(self.table[self.trial_counter,14])
self.tgtscy=float(self.table[self.trial_counter,15])
self.tgtscz=float(self.table[self.trial_counter,16])
tgttsx=float(self.table[self.trial_counter,11])
tgttsy=float(self.table[self.trial_counter,12])
tgttsz=float(self.table[self.trial_counter,13])
tgtrx=float(self.table[self.trial_counter,17])
tgtry=float(self.table[self.trial_counter,18])
tgtrz=float(self.table[self.trial_counter,19])
if self.hand == 'Left':
tgttsx *= -1
tgtrx *= -1
self.static_task = (str(self.table[self.trial_counter,5]) == "True")
self.target_model = str(self.table[self.trial_counter,6])
self.highlighted_indices=[int(s)-1 for s in self.table[self.trial_counter,4].split(' ')]
if self.hand == 'Left':
self.highlighted_indices=[4-i for i in self.highlighted_indices]
#NOTE:this position finding is present in all three tasks
# it is somewhat hacky and can be improved upon
self.tgtposx = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][[-2,-1],0]) + tgttsx
self.tgtposy = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][[0,1],1]) + tgttsy
if len(self.highlighted_indices) > 3: #for sphere, return to just average of all fingers
self.tgtposx = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][:,0]) + tgttsx
#self.tgtposy = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][:,1])
self.tgtposz = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][:,2]) + tgttsz
self.tar = self.loader.loadModel(self.target_model)
self.tar.setScale(self.tgtscx,self.tgtscy,self.tgtscz)
self.tar.setPos(self.tgtposx,self.tgtposy,self.tgtposz)
self.tar.setHpr(tgtrx,tgtry,tgtrz)
self.tar.setColorScale(0.1, 0.1, 0.1, 1)
self.tar.setAlphaScale(0.7)
self.tar.setTransparency(TransparencyAttrib.MAlpha)
self.tar.reparentTo(self.render)
self.tar.hide()
self.delay=float(self.table[self.trial_counter,8])
self.loc_angle=math.radians(float(self.table[self.trial_counter,9]))
self.invf=float(self.table[self.trial_counter,10])
self.distances = [[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]]
##############
#MOVE FINGERS#
##############
def read_data(self,task):
error, data = self.dev.read()
if data is not None:
data *= 0.001
self.ts = data.time
data = np.dot(data,self.rotmat)
self.data = data
if self.med_data is None:
self.med_data = np.median(data, axis=0)
if self.space:
self.statenum.extend(([self.checkstate()])*len(data.time))
return task.cont
def move_player(self,p,task):
if self.data is not None :
k = p*3
new_x = 10*np.mean(self.data[-1,k]) + self.player_offsets[p][0] - 10*self.med_data[k]
new_y = 10*np.mean(self.data[-1,k + 1]) + self.player_offsets[p][1] - 10*self.med_data[k + 1]
new_z = 10*np.mean(self.data[-1,k + 2]) + self.player_offsets[p][2] - 10*self.med_data[k + 2]
#make sure digits do not cross each other
if ((p in range(1,3) and p+1 in self.highlighted_indices and new_x > self.players[p+1].getX())
or (p in range(2,4) and p-1 in self.highlighted_indices and new_x < self.players[p-1].getX())):
new_x = self.players[p].getX()
#make sure digits do not cross into target
if self.space == True and p in self.highlighted_indices:
self.distances[p][0] = new_x - self.tar.getX()
self.distances[p][1] = new_y - self.tar.getY()
self.distances[p][2] = new_z - self.tar.getZ()
self.check_pos(p)
self.players[p].setPos(new_x, new_y, new_z)
return task.cont
def check_pos(self,p):
if (abs(self.distances[p][0]) < self.invf*self.tgtscx
and abs(self.distances[p][1]) < self.invf*self.tgtscy
and abs(self.distances[p][2]) < self.invf*self.tgtscz):
self.too_long(p)
##################
#CHECK COMPLETION#
##################
def close_to_target(self):
for i in self.highlighted_indices:
if self.contacts[i] == False:
return False
if not self.check_angle():
self.tar.setColorScale(0.1,0.1,0.1,1)
self.tar.setAlphaScale(0.7)
return False
self.tar.setColorScale(0,1,1,1)
return True
def check_hold(self):
if not self.close_to_target():
self.hold_timer.reset(0.5)
return False
return self.hold_timer.elapsed() < 0
def check_angle(self):
#hardcoded condition may be able to be assimilated into old angle method somehow
if self.table[self.trial_counter,1] == 'pts':
vec = np.subtract(self.players[self.highlighted_indices[1]].getPos(), self.players[self.highlighted_indices[0]].getPos())
xvec = [1,0,0]
if self.hand == 'Left':
xvec = [-1,0,0]
print(math.degrees(math.acos(np.dot(vec,xvec)/(np.linalg.norm(vec)*np.linalg.norm(xvec)))))
if math.acos(np.dot(vec,xvec)/(np.linalg.norm(vec)*np.linalg.norm(xvec))) < self.loc_angle:
return True
else: return False
thumbindx = 0
if self.hand == "Left": thumbindx = 4
for i in self.highlighted_indices:
if i == thumbindx:
continue
th = self.distances[thumbindx]
fg = self.distances[i]
if math.acos(np.dot(th,fg)/(np.linalg.norm(th)*np.linalg.norm(fg))) > self.loc_angle:
return True
return False
def adjust_targets(self):
#no adjustment if more than 2 fingers or position is prone
if len(self.highlighted_indices) > 2 or self.wrist == 'pron':
return
xadj,yadj,zadj = np.mean(np.asarray(self.distances)[self.highlighted_indices],0)
#yadj = np.mean(np.asarray(self.distances)[self.highlighted_indices][1])
#zadj = np.mean(np.asarray(self.distances)[self.highlighted_indices][2])
#do adjustment on all tasks with same name
if self.hand == 'Left':
xadj = -xadj
for i in range(self.trial_counter+1,self.table.shape[0]):
if self.table[i,1] == self.table[self.trial_counter,1]:
self.table[i,11] = float(self.table[i,11]) + xadj
self.table[i,12] = float(self.table[i,12]) + yadj
self.table[i,13] = float(self.table[i,13]) + zadj
#########
#LOGGING#
#########
def play_success(self):
if int(self.block) == 0:
self.adjust_targets()
self.pop.play()
self.tar.hide()
self.highlighted_indices = [0,1,2,3,4]
def log_text(self):
self.bgtext.setText('Now logging...')
def log_data(self, task):
if (self.trial_counter + 1) <= self.table.shape[0]:
if self.space:
self.log_file_name = os.path.join(self.grip_dir,
self.sub_id+"_"+self.sess_id+"_"+self.hand+"_"+
str(self.table[self.trial_counter,1])+"_"+str(self.table[self.trial_counter,0])+".csv" )
self.movvars = np.column_stack((self.ts, self.statenum, self.data))
self.statenum = []
if self.mode=='task':
with open(self.log_file_name, 'ab') as f:
np.savetxt(f, self.movvars, fmt='%10.5f', delimiter=',')
return task.cont
else:
pass
def stoplog_text(self):
self.dirtext.clearText()
self.bgtext.setText('Done logging!')
for i in range(5):
self.players[i].show()
#######
#RESET#
#######
def delete_file(self):
if (self.trial_counter + 1) <= self.table.shape[0]:
self.log_file_name = os.path.join(self.grip_dir,
self.sub_id+"_"+self.sess_id+"_"+self.hand+"_"+
str(self.table[self.trial_counter,1])+"_"+str(self.table[self.trial_counter,0])+".csv" )
try:
os.remove(self.log_file_name)
except OSError:
pass
else:
pass
def reset_baseline(self):
self.med_data = None
def reset_keyboard_bool(self):
self.space = False
def hide_target(self):
self.tar.hide()
self.intoObject.removeNode()
self.imageObject.destroy()
def update_trial_command(self):
self.dirtext.setText(str(self.table[self.trial_counter,2]))
if self.hand == 'Left':
xfac = -0.25
else:
xfac = 0.25
self.imageObject = OnscreenImage(image = str(self.table[self.trial_counter,3]),scale=(xfac,0.25,0.25),pos=(-1.2, 0, 0.3))
def increment_trial_counter(self):
self.trial_counter += 1
self.update_trial_command()
########
#TIMERS#
########
def start_trial_countdown(self):
self.countdown_timer.reset(self.max_time)
def start_hold_countdown(self):
self.hold_timer.reset(0.5)
def start_post_countdown(self):
self.countdown_timer.reset(2)
def time_elapsed(self):
return self.countdown_timer.elapsed() < 0
#########
#MACHINE#
#########
def update_state(self, task):
self.step()
return task.cont
def wait_for_space(self):
return self.space
def space_on(self):
self.space = True
#####
#END#
#####
def trial_counter_exceeded(self):
return (self.trial_counter+1) > self.table.shape[0]-1
def clean_up(self):
#write last known positions to 'final_targets' file
f = open('src/pinch_task/trialtable_flex.csv')
header = f.readline().rstrip()
np.savetxt(self.grip_dir + '/final_targets.csv',self.table,fmt='%s',header = header, delimiter=',')
f.close()
sys.exit()
########
#SERIAL#
########
def init_ser(self):
#CONNECT TO HAPTIC DEVICE
ports = list_ports.comports()
mydev = next((p.device for p in ports if p.pid == 1155))
self.ser = serial.Serial(mydev,9600,timeout=.1)
def sendsig(self,signal,finger,intensity):
#would read from table, but table not available yet
if intensity > 255:
intensity = 255
dur_int = [signal,finger,intensity]
data = struct.pack('3B',*dur_int)
self.ser.write(data)
class TunnelPinchTask(ShowBase, GripStateMachine):
DATA_DIR = 'data'
def __init__(self, id, session, hand, block, mode, wrist):
ShowBase.__init__(self)
GripStateMachine.__init__(self)
base.disableMouse()
wp = WindowProperties()
wp.setSize(1920,1080)
wp.setFullscreen(True)
wp.setUndecorated(True)
base.win.requestProperties(wp)
self.sub_id = str(id)
self.sess_id = str(session)
self.hand = str(hand)
self.block = str(block)
self.mode = str(mode)
self.wrist = str(wrist)
self.prev_blk = os.path.join(self.DATA_DIR,'exp_2',self.sub_id,self.sess_id,self.wrist,self.hand,"B0")
self.exp_blk0 = os.path.join(self.DATA_DIR,'exp_1',self.sub_id,self.sess_id,self.wrist,self.hand,"B0")
self.table = np.loadtxt('src/tunnel_pinch_task/trialtable_flex.csv',dtype='str',delimiter=',',skiprows=1)
indices = {}
try:
self.prev_table = np.loadtxt(os.path.join(self.prev_blk, 'final_targets.csv'),dtype='str',delimiter=',',skiprows=1)
except:
try:
self.prev_table = np.loadtxt(os.path.join(self.exp_blk0, 'final_targets.csv'),dtype='str',delimiter=',',skiprows=1)
except:
print('Previous target file not found, results may be suboptimal')
try:
for i in range(self.prev_table.shape[0]):
indices[self.prev_table[i,1]] = int(self.prev_table[i,0])-1
for i in range(self.table.shape[0]):
self.table[i,11] = self.prev_table[indices[self.table[i,1].strip()],11]
self.table[i,12] = self.prev_table[indices[self.table[i,1].strip()],12]
self.table[i,13] = self.prev_table[indices[self.table[i,1].strip()],13]
except:
print('Invalid target file')
self.table = np.array([[item.strip() for item in s] for s in self.table])
###################################################
#only use rows relevant to this block
#HARDCODED! NOTE IN LOG SHEET
spec_table = []
for i in range(self.table.shape[0]):
if int(self.block)%5 == 0: #block 0 to adjust positions
if "(p)" in self.table[i,2]:
spec_table.append(self.table[i])
elif int(self.block)%5 == 1:
if "(L)" in self.table[i,2]:
spec_table.append(self.table[i])
elif int(self.block)%5 == 2:
if "(L+t)" in self.table[i,2]:
spec_table.append(self.table[i])
elif int(self.block)%5 == 3:
if "(S)" in self.table[i,2]:
spec_table.append(self.table[i])
elif int(self.block)%5 == 4:
if "(S+t)" in self.table[i,2]:
spec_table.append(self.table[i])
###################################################
self.table = np.array(spec_table)
self.session_dir = os.path.join(self.DATA_DIR,'exp_2',self.sub_id,self.sess_id,self.wrist,self.hand)
self.subjinfo = self.sub_id + '_' + self.sess_id + '_' + self.hand + '_log.yml'
self.p_x,self.p_y,self.p_a = GET_POS(self.session_dir,self.subjinfo,self.hand,self.wrist)
self.rotmat = ROT_MAT(self.p_a,self.hand)
self.setup_text()
self.setup_lights()
self.setup_camera()
self.trial_counter = 0
self.load_models()
self.load_audio()
self.update_trial_command()
self.countdown_timer = CountdownTimer()
self.hold_timer = CountdownTimer()
self.cTrav = CollisionTraverser()
self.chandler = CollisionHandlerEvent()
self.chandler.addInPattern('%fn-into-%in')
self.chandler.addOutPattern('%fn-outof-%in')
self.chandler.addAgainPattern('%fn-again-%in')
self.attachcollnodes()
taskMgr.add(self.read_data, 'read')
for i in range(5):
taskMgr.add(self.move_player, 'move%d' % i, extraArgs = [i], appendTask=True)
taskMgr.add(self.log_data, 'log_data')
taskMgr.add(self.update_state, 'update_state', sort=1)
self.accept('space', self.space_on)
self.accept('escape', self.clean_up)
self.space = False
self.statenum = list()
self.max_time = 20
self.med_data = None
self.grip_dir = os.path.join(self.DATA_DIR,'exp_2',self.sub_id,self.sess_id,self.wrist,self.hand,"B"+self.block)
if not os.path.exists(self.grip_dir):
print('Making new folders: ' + self.grip_dir)
os.makedirs(self.grip_dir)
self.dev = MpDevice(RightHand(calibration_files=['calibs/cal_mat_70_v2.mat',
'calibs/cal_mat_73_v2.mat',
'calibs/cal_mat_56.mat',
'calibs/cal_mat_58_v2.mat',
'calibs/cal_mat_50.mat'], clock=mono_clock.get_time))
############
#SET UP HUD#
############
def setup_text(self):
self.bgtext = OnscreenText(text='Not recording.', pos=(-0.8, 0.8),
scale=0.08, fg=(0, 0, 0, 1),
bg=(1, 1, 1, 1), frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.bgtext.reparentTo(self.aspect2d)
self.dirtext = OnscreenText( pos=(-0.6, 0.65),
scale=0.08, fg=(0, 0, 0, 1),
bg=(1, 1, 1, 1), frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.dirtext.reparentTo(self.aspect2d)
##########################
#SET UP SCENE AND PLAYERS#
##########################
def setup_lights(self):
pl = PointLight('pl')
pl.setColor((1, 1, 1, 1))
plNP = self.render.attachNewNode(pl)
plNP.setPos(-10, -10, 10)
self.render.setLight(plNP)
pos = [[[0, 0, 50], [0, 0, -10]],
[[0, -50, 0], [0, 10, 0]],
[[-50, 0, 0], [10, 0, 0]]]
for i in pos:
dl = Spotlight('dl')
dl.setColor((1, 1, 1, 1))
dlNP = self.render.attachNewNode(dl)
dlNP.setPos(*i[0])
dlNP.lookAt(*i[1])
dlNP.node().setShadowCaster(False)
self.render.setLight(dlNP)
def setup_camera(self):
self.cam.setPos(0, 0, 12)
self.cam.lookAt(0, 2, 0)
self.camLens.setFov(90)
def load_models(self):
self.back_model = self.loader.loadModel('models/back')
self.back_model.setScale(10, 10, 10)
if self.hand == "Left":
self.back_model.setH(90)
self.back_model.reparentTo(self.render)
self.player_offsets = [[self.p_x[0]-5, self.p_y[0]+3, 0], [self.p_x[1]-2.5, self.p_y[1]+4.5, 0], [self.p_x[2], self.p_y[2]+5, 0],
[self.p_x[3]+2.5, self.p_y[3]+4.5, 0], [self.p_x[4]+5, self.p_y[4]+3, 0]]
self.p_col =[[0,0,250],[50,0,200],[125,0,125],[200,0,50],[250,0,0]]
if self.hand == 'Left':
self.p_col = self.p_col[::-1]
self.players = list()
self.contacts = list()
for counter, value in enumerate(self.player_offsets):
self.players.append(self.loader.loadModel('models/target'))
self.contacts.append(False)
self.players[counter].setPos(*value)
self.players[counter].setScale(0.2, 0.2, 0.2)
self.players[counter].setColorScale(
self.p_col[counter][0]/255, self.p_col[counter][1]/255, self.p_col[counter][2]/255, 1)
self.players[counter].reparentTo(self.render)
self.players[counter].show()
self.target_select()
def load_audio(self):
self.pop = self.loader.loadSfx('audio/Blop.wav')
self.buzz = self.loader.loadSfx('audio/Buzzer.wav')
############################
#SET UP COLLISION MECHANICS#
############################
def attachcollnodes(self):
self.inside = [False]*5
for i in range(5):
self.fromObject = self.players[i].attachNewNode(CollisionNode('colfromNode'+str(i)))
self.fromObject.node().addSolid(CollisionSphere(0,0,0,1))
self.cTrav.addCollider(self.fromObject, self.chandler)
for i in range(5):
self.accept('colfromNode%d-into-colintoNode' % i, self.collide1,[i])
self.accept('colfromNode%d-again-colintoNode' % i, self.collide2,[i])
self.accept('colfromNode%d-outof-colintoNode' % i, self.collide3,[i])
def collide1(self,f,collEntry):
if f in self.highlighted_indices:
self.players[f].setColorScale(0,1,0,1)
self.tar.setColorScale(0.2,0.2,0.2,1)
self.tar.setAlphaScale(0.7)
self.contacts[f] = True
taskMgr.doMethodLater(self.delay,self.too_long,'too_long%d' % f,extraArgs = [f])
def collide2(self,f,collEntry):
for i in self.highlighted_indices:
if self.contacts[i] == False:
return
taskMgr.remove('too_long%d' % f)
def collide3(self,f,collEntry):
taskMgr.remove('too_long%d' % f)
self.reset_fing(f)
self.tar.setColorScale(0.1,0.1,0.1,1)
self.tar.setAlphaScale(0.7)
def too_long(self,f):
self.reset_fing(f)
self.tar.setColorScale(0.5,0.2,0.2,1)
self.tar.setAlphaScale(0.7)
def reset_fing(self,f):
self.players[f].setColorScale(
self.p_col[f][0]/255, self.p_col[f][1]/255, self.p_col[f][2]/255, 1)
self.contacts[f] = False
###############
#TARGET THINGS#
###############
def show_target(self):
self.target_select()
self.intoObject = self.tar.attachNewNode(CollisionNode('colintoNode'))
if self.table[self.trial_counter,7] == "sphere":
self.intoObject.node().addSolid(CollisionSphere(0,0,0,1))
elif self.table[self.trial_counter,7] == "cylinder":
self.intoObject.node().addSolid(CollisionTube(0,0,-2,0,0,2,1))
else:
raise NameError("No such collision type")
self.tar.show()
self.occSolid.show()
self.occLines.show()
for i in range(5):
if i not in self.highlighted_indices:
self.players[i].hide()
def target_select(self):
self.tgtscx=float(self.table[self.trial_counter,14])
self.tgtscy=float(self.table[self.trial_counter,15])
self.tgtscz=float(self.table[self.trial_counter,16])
tgttsx=float(self.table[self.trial_counter,11])
tgttsy=float(self.table[self.trial_counter,12])
tgttsz=float(self.table[self.trial_counter,13])
tgtrx=float(self.table[self.trial_counter,17])
tgtry=float(self.table[self.trial_counter,18])
tgtrz=float(self.table[self.trial_counter,19])
if self.hand == 'Left':
tgttsx *= -1
tgtrx *= -1
self.static_task = (str(self.table[self.trial_counter,5]) == "True")
self.target_model = str(self.table[self.trial_counter,6])
self.highlighted_indices=[int(s)-1 for s in self.table[self.trial_counter,4].split(' ')]
if self.hand == 'Left':
self.highlighted_indices=[4-i for i in self.highlighted_indices]
self.tgtposx = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][[-2,-1],0]) + tgttsx
self.tgtposy = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][[0,1],1]) + tgttsy
if self.hand == 'Left':
self.tgtposx = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][[0,1],0]) + tgttsx
self.tgtposy = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][[-2,-1],1]) + tgttsy
#self.tgtposx = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][:,0])
#self.tgtposy = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][:,1])
self.tgtposz = np.mean(np.asarray(self.player_offsets)[self.highlighted_indices][:,2]) + tgttsz
self.tar = self.loader.loadModel(self.target_model)
self.tar.setScale(self.tgtscx,self.tgtscy,self.tgtscz)
self.tar.setPos(self.tgtposx,self.tgtposy,self.tgtposz)
self.tar.setHpr(tgtrx,tgtry,tgtrz)
self.tar.setColorScale(0.1, 0.1, 0.1, 1)
self.tar.setAlphaScale(0.7)
self.tar.setTransparency(TransparencyAttrib.MAlpha)
self.tar.reparentTo(self.render)
self.tar.hide()
if len(self.highlighted_indices) == 2:
dx = self.players[self.highlighted_indices[0]].getX() - self.players[self.highlighted_indices[1]].getX()
dy = self.players[self.highlighted_indices[0]].getY() - self.players[self.highlighted_indices[1]].getY()
angle = math.degrees(math.atan(dy/dx))
self.table[self.trial_counter,9] = str(angle) + ' ' + str(angle-180)
self.angs=self.table[self.trial_counter,9].split(' ')
self.angs = [float(a) for a in self.angs]
self.tunn_width=float(self.table[self.trial_counter,10])
self.r = 1.5
if int(self.block) == 0:
self.r = 0
self.x = [self.r*math.cos(math.radians(a)) for a in self.angs]
self.y = [self.r*math.sin(math.radians(a)) for a in self.angs]
self.occ = draw_shape(self.angs,self.tunn_width,self.r)
self.occSolid = render.attachNewNode(self.occ[0])
self.occSolid.setPos(self.tgtposx,self.tgtposy,self.tgtposz)
self.occSolid.setColorScale(0,1,1,0)
self.occSolid.setTransparency(TransparencyAttrib.MAlpha)
self.occSolid.setAlphaScale(0.6)
self.occLines = render.attachNewNode(self.occ[1])
self.occLines.setPos(self.tgtposx,self.tgtposy,self.tgtposz)
self.occSolid.hide()
self.occLines.hide()
self.delay=float(self.table[self.trial_counter,8])
self.distances = [[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]]
#change camera to be on top of target
self.cam.setPos(self.tgtposx, self.tgtposy - 2, 12)
self.back_model.setPos(self.tgtposx,self.tgtposy - 2,0)
self.cam.lookAt(self.tgtposx, self.tgtposy, 0)
##############
#MOVE FINGERS#
##############
def read_data(self,task):
error, data = self.dev.read()
if data is not None:
data *= 0.001
self.ts = data.time
data = np.dot(data,self.rotmat)
self.data = data
if self.med_data is None:
self.med_data = np.median(data, axis=0)
if self.space:
self.statenum.extend(([self.checkstate()])*len(data.time))
return task.cont
def move_player(self,p,task):
if self.data is not None :
k = p*3
new_x = 10*np.mean(self.data[-1,k]) + self.player_offsets[p][0] - 10*self.med_data[k]
new_y = 10*np.mean(self.data[-1,k + 1]) + self.player_offsets[p][1] - 10*self.med_data[k + 1]
new_z = 10*np.mean(self.data[-1,k + 2]) + self.player_offsets[p][2] - 10*self.med_data[k + 2]
#make sure digits do not cross each other
if ((p in range(1,3) and p+1 in self.highlighted_indices and new_x > self.players[p+1].getX())
or (p in range(2,4) and p-1 in self.highlighted_indices and new_x < self.players[p-1].getX())):
new_x = self.players[p].getX()
#make sure digits do not cross into target
if self.space == True and p in self.highlighted_indices:
self.distances[p][0] = new_x - self.tar.getX()
self.distances[p][1] = new_y - self.tar.getY()
self.distances[p][2] = new_z - self.tar.getZ()
self.check_pos(p)
self.players[p].setPos(new_x, new_y, new_z)
return task.cont
def check_pos(self, p):
x = self.distances[p][0]
y = self.distances[p][1]
z = self.distances[p][2]
hit = True
for i in range(len(self.angs)):
p_ang = math.acos((x*self.x[i]+y*self.y[i])/(self.r*(x**2+y**2)**0.5))
if math.sin(p_ang)*(x**2+y**2)**0.5 < self.tunn_width and p_ang < math.pi/2:
hit = False
break
if (abs(z) <= 1.2 #check z location
and x**2 + y**2 <= self.r**2 #within radius of circle
and hit == True):
if self.inside[p] is False:
self.ignore('colfromNode%d-into-colintoNode' % p)
self.ignore('colfromNode%d-again-colintoNode' % p)
self.players[p].setColorScale(1,1,0,1)
self.inside[p] = True
else:
if self.inside[p] is True and x**2 + y**2 > self.r**2:
self.accept('colfromNode%d-into-colintoNode' % p, self.collide1,[p])
self.accept('colfromNode%d-again-colintoNode' % p, self.collide2,[p])
self.players[p].setColorScale(
self.p_col[p][0]/255, self.p_col[p][1]/255, self.p_col[p][2]/255, 1)
self.inside[p] = False
##################
#CHECK COMPLETION#
##################
def close_to_target(self):
for i in self.highlighted_indices:
if self.contacts[i] == False:
return False
self.tar.setColorScale(0,1,1,1)
return True
def check_hold(self):
if not self.close_to_target():
self.hold_timer.reset(0.5)
return False
return self.hold_timer.elapsed() < 0
def adjust_targets(self):
#no adjustment if more than 2 fingers or position is prone
if len(self.highlighted_indices) > 2 or self.wrist == 'pron':
return
xadj,yadj,zadj = np.mean(np.asarray(self.distances)[self.highlighted_indices],0)
#xadj = np.mean(np.asarray(self.distances)[self.highlighted_indices][0])
#yadj = np.mean(np.asarray(self.distances)[self.highlighted_indices][1])
#zadj = np.mean(np.asarray(self.distances)[self.highlighted_indices][2])
#do adjustment on all tasks with same name
if self.hand == 'Left':
xadj = -xadj
for i in range(self.trial_counter+1,self.table.shape[0]):
if self.table[i,1] == self.table[self.trial_counter,1]:
self.table[i,11] = float(self.table[i,11]) + xadj
self.table[i,12] = float(self.table[i,12]) + yadj
self.table[i,13] = float(self.table[i,13]) + zadj
#########
#LOGGING#
#########
def play_success(self):
if int(self.block) == 0:
self.adjust_targets()
self.pop.play()
self.tar.hide()
self.highlighted_indices = [0,1,2,3,4]
def log_text(self):
self.bgtext.setText('Now logging...')
def log_data(self, task):
if (self.trial_counter + 1) <= self.table.shape[0]:
if self.space:
self.log_file_name = os.path.join(self.grip_dir,
self.sub_id+"_"+self.sess_id+"_"+self.hand+"_"+
str(self.table[self.trial_counter,1])+"_"+str(self.table[self.trial_counter,0])+".csv" )
self.movvars = np.column_stack((self.ts, self.statenum, self.data))
self.statenum = []
if self.mode=='task':
with open(self.log_file_name, 'ab') as f:
np.savetxt(f, self.movvars, fmt='%10.5f', delimiter=',')
return task.cont
else:
pass
def stoplog_text(self):
self.dirtext.clearText()
self.bgtext.setText('Done logging!')
for i in range(5):
self.players[i].show()
#######
#RESET#
#######
def delete_file(self):
if (self.trial_counter + 1) <= self.table.shape[0]:
self.log_file_name = os.path.join(self.grip_dir,
self.sub_id+"_"+self.sess_id+"_"+self.hand+"_"+
str(self.table[self.trial_counter,1])+"_"+str(self.table[self.trial_counter,0])+".csv" )
try:
os.remove(self.log_file_name)
except OSError:
pass
else:
pass
def reset_baseline(self):
self.med_data = None
def reset_keyboard_bool(self):
self.space = False
def hide_target(self):
self.tar.hide()
self.occSolid.hide()
self.occLines.hide()
self.intoObject.removeNode()
self.imageObject.destroy()
def update_trial_command(self):
self.dirtext.setText(str(self.table[self.trial_counter,2]))
if self.hand == 'Left':
xfac = -0.25
else:
xfac = 0.25
self.imageObject = OnscreenImage(image = str(self.table[self.trial_counter,3]),scale=(xfac,0.25,0.25),pos=(-0.8, 0, 0.3))
def increment_trial_counter(self):
self.trial_counter += 1
self.update_trial_command()
########
#TIMERS#
########
def start_trial_countdown(self):
self.countdown_timer.reset(self.max_time)
def start_hold_countdown(self):
self.hold_timer.reset(0.5)
def start_post_countdown(self):
self.countdown_timer.reset(2)
def time_elapsed(self):
return self.countdown_timer.elapsed() < 0
#########
#MACHINE#
#########
def update_state(self, task):
self.step()
return task.cont
def wait_for_space(self):
return self.space
def space_on(self):
self.space = True
#####
#END#
#####
def trial_counter_exceeded(self):
return (self.trial_counter+1) > self.table.shape[0]-1
def clean_up(self):
#write last known positions to 'final_targets' file
f = open('src/pinch_task/trialtable_flex.csv')
header = f.readline().rstrip()
np.savetxt(self.grip_dir + '/final_targets.csv',self.table,fmt='%s',header = header, delimiter=',')
f.close()
sys.exit()
# TODO: apply calibration?
time = self.clock()
return time, self._buffer
def exit(self):
self._device.stop()
self._device.close()
if __name__ == '__main__':
import time
from toon.input import MpDevice
dev = MpDevice(ForceKeyboard())
times = []
with dev:
start = time.time()
while time.time() - start < 240:
dat = dev.read()
if dat is not None:
time, data = dat
print(data)
times.append(time)
time.sleep(0.016)
times = np.hstack(times)
import matplotlib.pyplot as plt
plt.plot(np.diff(times))
plt.show()
class HandS(ShowBase, HandSetupMachine):
def __init__(self, id, session, hand, wrist, trial, exp):
self.sub_id = id
self.trialid = trial
self.session = session
self.hand = hand
self.wrist = wrist
self.exp = exp
self.DATA_DIR = 'data'
ShowBase.__init__(self)
HandSetupMachine.__init__(self)
props = WindowProperties()
props.setTitle('Hand Setup')
self.session_dir = os.path.join(self.DATA_DIR, 'exp_' + self.exp,
self.sub_id, self.session, self.wrist,
self.hand, 'Setup_' + self.trialid)
if not os.path.exists(self.session_dir):
print('Making new folders: ' + self.session_dir)
os.makedirs(self.session_dir)
self.win.requestProperties(props)
self.render.setAntialias(AntialiasAttrib.MMultisample)
self.render.setShaderAuto() # allows shadows
self.setFrameRateMeter(True)
self.space = False
self.dev = MpDevice(
RightHand(
calibration_files=[
'calibs/cal_mat_15.mat', # thumb
'calibs/cal_mat_31.mat',
'calibs/cal_mat_13.mat',
'calibs/cal_mat_21.mat',
'calibs/cal_mat_8.mat'
],
clock=mono_clock.get_time))
#self.finger = int(finger) * 3 # offset
#self.f2 = int(finger)
self.disableMouse()
self.countdown_timer = CountdownTimer()
# self.table = pd.read_table(trial_table) # trial table
self.setup_lights()
self.setup_camera()
self.load_models()
# add tasks (run every frame)
taskMgr.add(self.get_user_input, 'move')
taskMgr.add(self.update_feedback_bar, 'update_feedback_bar', sort=1)
self.accept('space', self.space_on) # toggle a boolean somewhere
self.accept('escape', self.clean_up)
self.med_data = None
self.noise1 = 0.0
self.noise2 = 0.0
self.noise3 = 0.0
self.noise4 = 0.0
self.noise5 = 0.0
self.trial_counter = 0
self.logging = False
self.text = OnscreenText(text='Not recording.',
pos=(-0.8, 0.7),
scale=0.08,
fg=(1, 1, 1, 1),
bg=(0, 0, 0, 1),
frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.button = DirectButton(text='Log data',
scale=0.05,
command=self.log_and_print,
pos=(-0.9, 0, 0.9))
self.button = DirectButton(text='Stop logging',
scale=0.05,
command=self.stop_logging,
pos=(-0.9, 0, 0.8))
def stop_logging(self):
self.logging = False
self.stoplog_text()
def log_and_print(self):
self.logging = True
self.trial_counter += 1
self.log_text()
self.t = Timer(30, self.incrementfile)
self.t.start()
def incrementfile(self):
if self.logging:
self.t.cancel()
self.log_and_print()
def load_models(self):
self.cam2dp.node().getDisplayRegion(0).setSort(-20)
OnscreenImage(parent=self.cam2dp, image='models/background.jpg')
self.bgtext = OnscreenText(pos=(-0.8, 0.8),
scale=0.08,
fg=(1, 1, 1, 1),
bg=(0, 0, 0, 1),
frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.bgtext.reparentTo(self.aspect2d)
self.errorlevel = OnscreenText(text='10%',
pos=(-0.99, -0.80),
scale=0.08,
fg=(1, 1, 1, 1),
bg=(0, 0, 0, 1),
frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.halfscale = OnscreenText(text='50%',
pos=(-0.99, -0.4),
scale=0.08,
fg=(1, 1, 1, 1),
bg=(0, 0, 0, 1),
frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.fullscale = OnscreenText(text='100%',
pos=(-0.99, 0.10),
scale=0.08,
fg=(1, 1, 1, 1),
bg=(0, 0, 0, 1),
frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.text = OnscreenText(text='Neutral position error',
pos=(-0.08, 0.8),
scale=0.08,
fg=(1, 1, 1, 1),
bg=(0, 0, 0, 1),
frame=(0.2, 0.2, 0.8, 1),
align=TextNode.ACenter)
self.text.reparentTo(self.aspect2d)
self.move_feedback = MeshDrawer2D()
self.move_feedback.setBudget(
100) # this is the number of triangles needed (two per rect)
feed_node = self.move_feedback.getRoot()
feed_node.setTwoSided(True)
feed_node.setDepthWrite(False)
feed_node.setTransparency(True)
feed_node.setBin('fixed', 0)
feed_node.setLightOff(True)
self.node = feed_node
self.node.reparentTo(self.render2d)
def setup_lights(self):
pl = PointLight('pl')
pl.setColor((1, 1, 1, 1))
plNP = self.render.attachNewNode(pl)
plNP.setPos(-0.5, -0.5, 0.5)
self.render.setLight(plNP)
pos = [[[0, 0, 3], [0, 0, -1]], [[0, -3, 0], [0, 1, 0]],
[[-3, 0, 0], [1, 0, 0]]]
for i in pos:
dl = Spotlight('dl')
dl.setColor((1, 1, 1, 1))
dlNP = self.render.attachNewNode(dl)
dlNP.setPos(*i[0])
dlNP.lookAt(*i[1])
dlNP.node().setShadowCaster(True)
self.render.setLight(dlNP)
def setup_camera(self):
self.cam.setPos(-2, -4, 2)
self.cam.lookAt(0, 0, 0)
def get_user_input(self, task):
error, data = self.dev.read()
if data is not None:
data *= 0.001
if self.med_data is None:
self.med_data = np.median(data, axis=0)
self.data = data
noise1 = np.sqrt(
np.square(data[-1, 0] - self.med_data[0]) +
np.square(data[-1, 1] - self.med_data[1]) +
np.square(data[-1, 2] - self.med_data[2]))
noise2 = np.sqrt(
np.square(data[-1, 3] - self.med_data[3]) +
np.square(data[-1, 4] - self.med_data[4]) +
np.square(data[-1, 5] - self.med_data[5]))
noise3 = np.sqrt(
np.square(data[-1, 6] - self.med_data[6]) +
np.square(data[-1, 7] - self.med_data[7]) +
np.square(data[-1, 8] - self.med_data[8]))
noise4 = np.sqrt(
np.square(data[-1, 9] - self.med_data[9]) +
np.square(data[-1, 10] - self.med_data[10]) +
np.square(data[-1, 11] - self.med_data[11]))
noise5 = np.sqrt(
np.square(data[-1, 12] - self.med_data[12]) +
np.square(data[-1, 13] - self.med_data[13]) +
np.square(data[-1, 14] - self.med_data[14]))
self.noise1 = noise1
self.noise2 = noise2
self.noise3 = noise3
self.noise4 = noise4
self.noise5 = noise5
if self.logging:
self.log_file_name = os.path.join(
self.session_dir,
"Setup_" + self.sub_id + "_" + self.session + "_" +
self.hand + "_" + str(self.trial_counter) + ".txt")
with open(self.log_file_name, 'ab') as f:
np.savetxt(f, self.data, fmt='%10.5f', delimiter=',')
else:
pass
return task.cont
def update_feedback_bar(self, task):
self.move_feedback.begin()
self.move_feedback.rectangle_raw(-0.5, -0.9, 0.1, self.noise1, 0, 0, 0,
0, Vec4(0.9, .2, .1, 1))
self.move_feedback.rectangle_raw(-0.525, -0.9, 0.15, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.rectangle_raw(-0.625, 0.1, 1.2, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.rectangle_raw(-0.625, -0.4, 0.07, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.rectangle_raw(-0.3, -0.9, 0.1, self.noise2, 0, 0, 0,
0, Vec4(0.9, .2, .1, 1))
self.move_feedback.rectangle_raw(-0.325, -0.9, 0.15, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.rectangle_raw(-0.1, -0.9, 0.1, self.noise3, 0, 0, 0,
0, Vec4(0.9, .2, .1, 1))
self.move_feedback.rectangle_raw(-0.125, -0.9, 0.15, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.rectangle_raw(0.1, -0.9, 0.1, self.noise4, 0, 0, 0,
0, Vec4(0.9, .2, .1, 1))
self.move_feedback.rectangle_raw(0.075, -0.9, 0.15, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.rectangle_raw(0.3, -0.9, 0.1, self.noise5, 0, 0, 0,
0, Vec4(0.9, .2, .1, 1))
self.move_feedback.rectangle_raw(0.275, -0.9, 0.15, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.rectangle_raw(-0.625, -0.80, 1.2, 0.015, 0, 0, 0, 0,
Vec4(1, 1, 1, 1))
self.move_feedback.end()
return task.cont
def space_on(self):
self.space = True
def update_state(self, task):
self.step()
return task.cont
# state machine functions
def wait_for_space(self):
return self.space
def log_text(self):
self.text.setText('Now logging...')
def stoplog_text(self):
self.text.setText('Paused logging!')
def time_elapsed(self):
return self.countdown_timer.elapsed() < 0
def post_text(self):
self.text.setText('Relax.')
def reset_baseline(self):
self.med_data = None
def clean_up(self):
sys.exit()
if __name__ == '__main__':
win = Win(vsync=1, screen=0)
height = win.height
cir = Circle(win, scale=0.1, fill_color=(0.2, 0.9, 0.7, 1))
cl = Circle(win, scale=0.025, fill_color=(1, 1, 1, 1))
cr = Circle(win, scale=0.025, fill_color=(0, 0, 0, 1))
dev = MpDevice(Mouse())
with dev:
t0 = default_timer()
t1 = t0 + 30
while t1 > default_timer():
res = dev.read()
if res:
time, data = res
left = data[data['id'] == 0]
right = data[data['id'] == 1]
if left.shape[0] > 0:
cir.position.x += float(sum(left['dy'])) / win.height
cl.position.x += float(sum(left['dx'])) / win.height
cl.position.y -= float(sum(left['dy'])) / win.height
if right.shape[0] > 0:
cir.position.y += float(sum(right['dx'])) / win.height
cr.position.x += float(sum(right['dx'])) / win.height
cr.position.y -= float(sum(right['dy'])) / win.height
cir.draw()
cl.draw()
cr.draw()
def enter(self):
ports = list_ports.comports()
mydev = next((p.device for p in ports if p.pid == 1155))
self._device = serial.Serial(mydev)
self._device.close()
sleep(0.5)
self._device.open()
return self
def exit(self, *args):
self._device.close()
def read(self):
data = self._device.read(64)
time = self.clock()
if data:
data = struct.unpack('<' + 'H' * 4, bytearray(data[:8]))
return self.Returns(pos=self.Pos(time, data))
else:
return None, None
if __name__ == '__main__':
from timeit import default_timer
dev = MpDevice(TestSerial())
with dev:
t1 = default_timer() + 5
while default_timer() < t1:
data = dev.read()
if data is not None:
print(data)
def test_err():
dev = MpDevice(Timebomb())
with dev:
sleep(0.2)
with raises(ValueError):
dat = dev.read()