def timer_event(self):
done = False
while not done:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, msg)
x = mdf.target[0]
y = mdf.target[1]
self.setTargetPos(x, y)
#print "x: ", x, "|", self.tgt2pix(x) , " y: ", y, "|", self.tgt2pix(y)
elif msg_type == rc.MT_ROBOT_CONTROL_SPACE_ACTUAL_STATE:
mdf = rc.MDF_ROBOT_CONTROL_SPACE_ACTUAL_STATE()
copy_from_msg(mdf, msg)
x = mdf.pos[0]
y = mdf.pos[1]
self.setCursorPos(x, y)
#print "x: ", mdf.pos[0], "y: ", mdf.pos[1]
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'CursorDisplay')
elif msg_type == MT_EXIT:
self.exit()
done = True
else:
done = True
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if self.calibrated:
if self.set_collecting:
if (msg_type == rc.MT_HOTSPOT_POSITION) & (self.got_coil == False):
# handling input message
in_mdf = rc.MDF_HOTSPOT_POSITION()
copy_from_msg(in_mdf, in_msg)
self.current_vtail = np.array(in_mdf.xyz[:]) #Hotspot position
self.current_vhead = np.array(in_mdf.ori[:3]) #Vector head of coil, used to find ori
self.got_coil = True
elif (msg_type == rc.MT_POLARIS_POSITION) & (self.got_head == False):
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = self.shuffle_q(np.array(in_mdf.ori[:]))
if in_mdf.tool_id == (self.glasses + 1):
# calculating output
self.head, Qr = self.tp.get_pos(orientations, positions)
print(self.head)
self.got_head = True
elif (self.got_head == True) & (self.got_coil == True):
plot_position = self.current_vtail - self.head + self.plot_vertex_vec
in_mdf = rc.MDF_POLARIS_POSITION()
out_mdf = rc.MDF_PLOT_POSITION()
copy_from_msg(in_mdf, in_msg)
out_mdf.xyz[:] = plot_position
out_mdf.ori[:] = np.append(self.current_vhead, 0)# Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")
self.count += 1
save_array = np.insert(np.concatenate(((self.current_vtail - self.head), self.current_vhead)), 0, self.count)
self.current_map_data = np.vstack((self.current_map_data, save_array))
self.got_coil = False
self.got_head = False
self.set_collecting = False
if self.count > 100:
location = raw_input("Finished! Where should it save?")
np.savetxt(str(location) +'.txt',self.current_map_data[1:],
delimiter=',', newline='/n')
mlab.savefig(str(location) + '.png', figure=self.fig)
mlab.close(self.fig)
self.count = 0
self.run()
def respond_to_ping(self, msg, module_name):
#print "PING received for '{0}'".format(p.module_name)
dest_mod_id = msg.GetHeader().dest_mod_id
p = rc.MDF_PING()
copy_from_msg(p, msg)
if (p.module_name.lower() == module_name.lower()) or (p.module_name == "*") or \
(dest_mod_id == self.mod.GetModuleID()):
mdf = rc.MDF_PING_ACK()
mdf.module_name = module_name + ":" + self.host_name # + ":" + self.host_os
msg_out = CMessage(rc.MT_PING_ACK)
copy_to_msg(mdf, msg_out)
self.mod.SendMessage(msg_out)
def get_frequency(self):
# loop over receiving messages until we get a POLARIS_POSITION message
# get a POLARIS_POSITION message, read sample_header.DeltaTime to get
# message frequency
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.001)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break;
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'CoilPlotter')
else:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(mdf, msg)
self.fsamp = 1/mdf.sample_header.DeltaTime
if self.fsamp != 0:
break
self.user_start_calibrate()
def process_message(self, msg):
# read a Dragonfly message
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
return
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'PlotHead')
elif msg_type == rc.MT_POLARIS_POSITION:
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, msg)
positions = np.asarray(in_mdf.xyz[:])
orientations = self.shuffle_q(np.asarray(in_mdf.ori[:]))
if in_mdf.tool_id == (self.pointer + 1):
Qf = qa.norm(orientations)
Qr = qa.mult(Qf, qa.inv(self.pointer_Qi)).flatten()
#find_nans(self.store_head, Qr, 'Qr')
Tk = positions
#find_nans(self.store_head, Tk, 'Tk')
tip_pos = (qa.rotate(Qr, self.pointer_Xi) + Tk).flatten()
self.pointer_position = np.append(self.pointer_position,
(tip_pos[np.newaxis, :]),
axis=0)
#self.pl.reset(x=self.pointer_position[:,0], y=self.pointer_position[:,1], z=self.pointer_position[:,2])
print("old=", tip_pos)
print("new=", self.tp.get_pos(orientations, positions)[0])
def process_message(self, msg):
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_INPUT_DOF_DATA:
mdf = rc.MDF_INPUT_DOF_DATA()
copy_from_msg(mdf, msg)
tag = mdf.tag
if tag in self.inputs.keys():
dof_vals = np.asarray(mdf.dof_vals[:], dtype=float)
cid = int(mdf.tag[-1])
pressed = ~self.was_pressed[cid] & (dof_vals > btn_threshold)
started = self.bounce_start[cid] > 0
# start timers on previously unstarted counters
self.bounce_start[cid, pressed & ~started] = time.time()
dt = time.time() - self.bounce_start[cid]
held = dt > bounce_threshold
valid_held = pressed & held
for vh in np.flatnonzero(valid_held):
if vh in name_lookup.keys():
self.was_pressed[cid, vh] = True
self.send_btn_press(name_lookup[vh], cid)
released = self.was_pressed[cid] & (dof_vals < btn_threshold)
valid_released = released & held & ~valid_held
for vr in np.flatnonzero(valid_released):
if vr in name_lookup.keys():
self.was_pressed[cid, vr] = False
self.send_btn_release(name_lookup[vr], cid)
self.bounce_start[cid, vr] = -1
def on_daq_callback(self, data):
mdf = rc.MDF_PLOT_POSITION()
self.serial_no += 1
mdf.tool_id = 0
mdf.missing = 0
self.variable += 1
mdf.xyz[:] = np.array([self.variable] * 3)
mdf.ori[:] = np.array(
[self.variable] * 4
) # will work but need!!! reading modules to know the format of buffer
#mdf.buffer[data.size:] = -1
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
print self.variable
sys.stdout.write('|')
sys.stdout.flush()
# now check for exit message
in_msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
hdr = msg.GetHeader()
msg_type = hdr.msg_type
dest_mod_id = hdr.dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print "Received MT_EXIT, disconnecting..."
self.daq_task.StopTask()
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
self.stop()
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'RandomGen')
def run(self):
while True:
msg = PyDragonfly.CMessage()
self.mod.ReadMessage(msg) # blocking read
print "Received message ", msg.GetHeader().msg_type
if msg.GetHeader().msg_type == rc.MT_UR5_MOVEMENT_COMMAND:
msg_data = rc.MDF_UR5_MOVEMENT_COMMAND()
copy_from_msg(msg_data, msg)
#position = np.frombuffer(msg_data.position)
#print " Data = [X: %d, Y: %d, Z: %f]" % \
# (msg_data.position[0], msg_data.position[1], msg_data.position[2])
movement_complete = self.ur5.send_movement_command(
msg_data.position, msg_data.max_velocity,
msg_data.acceleration)
if movement_complete:
# send movement complete message
self.mod.SendSignal(rc.MT_UR5_MOVEMENT_COMPLETE)
else:
# print ur5 error message
print "No movement complete received."
elif msg.GetHeader().msg_type == rc.MT_UR5_REQUEST_CONNECTED:
if self.ur5.connected:
self.mod.SendSignal(rc.MT_UR5_CONNECTED)
# ideally we'd capture not connected, but not today...
elif msg.GetHeader().msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'UR5Control')
def timer_event(self):
done = False
while not done:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_TASK_STATE_CONFIG:
self.tsc_mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(self.tsc_mdf, msg)
elif msg_type == rc.MT_FORCE_FEEDBACK:
mdf = rc.MDF_FORCE_FEEDBACK()
copy_from_msg(mdf, msg)
#self.fdbk_actual_pos = []
self.fdbk_actual_pos = [mdf.x, mdf.y, mdf.z, 0.0, 0.0, 0.0]
self.update_judging_data()
elif msg_type == rc.MT_FORCE_SENSOR_DATA:
mdf = rc.MDF_FORCE_SENSOR_DATA()
copy_from_msg(mdf, msg)
self.fdbk_actual_pos = []
self.fdbk_actual_pos.extend(mdf.data)
self.update_judging_data()
elif msg_type == rc.MT_END_TASK_STATE:
self.ets_mdf = rc.MDF_END_TASK_STATE()
copy_from_msg(self.ets_mdf, msg)
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'SimpleDisplay')
elif msg_type == MT_EXIT:
self.exit()
done = True
else:
done = True
self.update_plot()
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
#print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing)))
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
if in_mdf.tool_id == (self.glasses + 1):
positions = np.array(in_mdf.xyz[:])
orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:])))
self.find_pos_to_glasses(positions, orientations)
def send_metronome(self, state):
mdf = rc.MDF_MNOME_STATE()
self.serial_no += 1
mdf.sample_header.SerialNo = self.serial_no
mdf.sample_header.Flags = 0
mdf.sample_header.DeltaTime = (1. / 5)
mdf.state = state
msg = CMessage(rc.MT_MNOME_STATE)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
print "Sent message %d" % state
def send_output(self, sample_header):
mdf = rc.MDF_COMPOSITE_MOVEMENT_COMMAND()
mdf.tag = 'composite'
vel = np.zeros_like(mdf.vel)
vel[:] = self.get_combined_command()
if self.idle_gateable == 1:
vel[:8] *= self.gate
mdf.vel[:] = vel
mdf.sample_header = sample_header
msg = CMessage(rc.MT_COMPOSITE_MOVEMENT_COMMAND)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
def send_sample_generated(self):
sg = rc.MDF_SAMPLE_GENERATED()
self.serial_no += 1
sg.sample_header.SerialNo = self.serial_no
sg.sample_header.Flags = 0
sg.sample_header.DeltaTime = (1. / self.freq)
sg.source_timestamp = self.default_timer() #time.time()
sg_msg = CMessage(rc.MT_SAMPLE_GENERATED)
copy_to_msg(sg, sg_msg)
self.mod.SendMessage(sg_msg)
sys.stdout.write('|')
sys.stdout.flush()
def process_message(self, in_msg):
# read a Dragonfly message
msg_type = in_msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:])))
if in_mdf.tool_id == (self.pointer + 1):
pointer_pos, Qr = self.pointer_tp.get_pos(
orientations, positions)
print pointer_pos
def send_btn_release(self, btn, controller_id):
print "controller_id %d sending button release %s" % (controller_id, btn)
btn_map = {'l1' : rc.PS3_B_L1,
'l2' : rc.PS3_B_L2,
'r1' : rc.PS3_B_R1,
'x' : rc.PS3_B_X,
'sq' : rc.PS3_B_SQUARE,
'crc': rc.PS3_B_CIRCLE,
'trg': rc.PS3_B_TRIANGLE}
mdf_out = rc.MDF_PS3_BUTTON_RELEASE()
mdf_out.whichButton = btn_map[btn]
mdf_out.controllerId = controller_id
msg_out = CMessage(rc.MT_PS3_BUTTON_RELEASE)
copy_to_msg(mdf_out, msg_out)
self.mod.SendMessage(msg_out)
def process_message(self, msg):
# read a Dragonfly message
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
return
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'PlotHead')
elif msg_type == rc.MT_PLOT_POSITION:
in_mdf = rc.MDF_PLOT_POSITION()
copy_from_msg(in_mdf, msg)
tail = np.array(in_mdf.xyz[:])*0.127 + (self.plot_vertex_vec)#Hotspot position
head = np.array(in_mdf.ori[:3])/4 #Vector head of coil, used to find ori
if np.any(np.isnan(tail)) == True:
pass
elif np.any(np.isnan(head)) == True:
pass
elif np.any(np.isinf(tail)) == True:
pass
elif np.any(np.isinf(head)) == True:
pass
else:
queue.put(np.vstack((head, tail)))
self.count=+1
print 'sent message'
elif msg_type == rc.MT_MNOME_STATE:
in_mdf = rc.MDF_MNOME_STATE()
copy_from_msg(in_mdf, msg)
if in_mdf.state == 0:
print 'got clear'
self.parent.reset = True
def send_btn_press(self, btn, controller_id):
print "controller_id %d sending button press %s" % (controller_id, btn)
btn_map = {'l1' : rc.PS3_B_L1,
'l2' : rc.PS3_B_L2,
'r1' : rc.PS3_B_R1,
'x' : rc.PS3_B_X,
'sq' : rc.PS3_B_SQUARE,
'crc': rc.PS3_B_CIRCLE,
'trg': rc.PS3_B_TRIANGLE}
mdf_out = rc.MDF_PS3_BUTTON_PRESS()
mdf_out.whichButton = btn_map[btn]
mdf_out.controllerId = controller_id
# make outgoing message data
msg_out = CMessage(rc.MT_PS3_BUTTON_PRESS)
copy_to_msg(mdf_out, msg_out)
self.mod.SendMessage(msg_out)
def run(self):
self.delta_time_calc = time.time() #time.time()
while True:
if (time.time() - self.delta_time_calc) % 2 == 0:
self.tail[0] = self.tail[0] + 1
self.head[1] = self.head[1] + 1
out_mdf = rc.MDF_PLOT_POSITION()
self.serial_no += 1
out_mdf.sample_header.SerialNo = self.serial_no
out_mdf.sample_header.Flags = 0
out_mdf.sample_header.DeltaTime = (1. / 5)
out_mdf.xyz[:] = self.tail
out_mdf.ori[:] = np.append(self.head, 0)# Qk - coil active orientation
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")
def calibrate_head(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.asarray(in_mdf.xyz[:])
orientations = self.shuffle_q(np.asarray(in_mdf.ori[:]))
#When arrays have been filled the calibration vector is generated
if (
(self.store_glasses >= self.store_glasses_pos.shape[0]) &
(self.store_glasses >= self.store_glasses_ori.shape[0]) &
(self.store_head >= self.store_head_pos.shape[0]) &
(self.store_head >= self.store_head_ori.shape[0])
):
self.calibrating = False
self.make_calibration_vector()
#Pointer is measured from ball 1, pointer end must be calculated
elif in_mdf.tool_id == (self.pointer + 1):
if self.store_head < self.store_head_pos.shape[0]:
if np.any(np.isnan(positions)) == True:
raise Exception, 'nan present'
elif np.any(np.isnan(orientations)) == True:
raise Exception, 'nan present'
Qf = qa.norm(orientations) #Sometimes gets nan, source unknown
if np.any(np.isnan(Qf)):
print(self.store_head, 'Qf', orientations)
#find_nans(self.store_head, Tk, 'Tk')
Cz_pos, Qr = self.pointer_tp.get_pos(orientations, positions)
#find_nans(self.store_head, Cz_pos, 'Cz')
self.store_head_pos[self.store_head, :] = Cz_pos
self.store_head_ori[self.store_head, :] = orientations
self.store_head += 1
elif in_mdf.tool_id == (self.glasses + 1):
if self.store_glasses < self.store_glasses_pos.shape[0]:
if np.any(np.isnan(positions)) == True:
raise Exception, 'nan present'
if np.any(np.isnan(orientations)) == True:
raise Exception, 'nan present'
self.store_glasses_pos[self.store_glasses, :] = positions
self.store_glasses_ori[self.store_glasses, :] = orientations
self.store_glasses += 1
def process_message(self, msg):
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == rc.MT_TMS_TRIGGER:
self.ext_trig.run()
self.TMS_trigger = True
else:
# if it is a NiDAQ message from channels 0-7, plot the data
#self.counter += 1
if msg_type == rc.MT_DAQ_DATA:
#sys.stdout.write("*")
#sys.stdout.flush()
mdf = rc.MDF_DAQ_DATA()
copy_from_msg(mdf, msg)
# add data to data buffers (necessary, or just use graphics buffers?)
# update plots to new data buffers
buf = mdf.buffer
self.new_data[:,:-self.config.perchan] = self.old_data[:,self.config.perchan:]
for i in xrange(self.config.nchan):
#if i == 0:
# print mdf.buffer[perchan * i:perchan * (i + 1)].size
self.new_data[i, -self.config.perchan:] = buf[i:self.config.nchan * self.config.perchan:self.config.nchan]
self.old_data[:] = self.new_data[:]
if self.parent.current_tab == 'Collect':
if self.TMS_trigger:
if self.config.pre_trig_samp <= np.argmax(self.old_data[self.config.trig_chan, :] >= 3) <= (self.config.pre_trig_samp)+200:
self.trig_index = np.argmax(self.old_data[self.config.trig_chan, :] >= 3)
self.collect_data = self.old_data[:self.config.nemg, self.trig_index - self.config.pre_trig_samp:self.trig_index + self.npt - self.config.pre_trig_samp]
self.old_data = self.old_data * 0
self.new_data = self.new_data * 0
self.new_collect_data = True
self.TMS_trigger = False
if self.parent.current_tab == 'Hotspot':
if self.config.pre_trig_samp <= np.argmax(self.old_data[self.config.trig_chan, :] >= 3) <= self.config.pre_trig_samp+200:
self.trig_index = np.argmax(self.old_data[self.config.trig_chan, :] >= 3)
self.hotspot_data = self.old_data[:self.config.nemg, self.trig_index - self.config.pre_trig_samp:self.trig_index + self.npt - self.config.pre_trig_samp]
self.new_hotspot_data = True
self.old_data = self.old_data * 0
self.new_data = self.new_data * 0
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.1)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_APP_START:
try:
mdf = rc.MDF_APP_START()
copy_from_msg(mdf, msg)
config = mdf.config
print "Config: %s" % config
# -- to do --
# get a list of all modules in appman.conf for this host
# see if any of the modules above are already/still running
# start non-running modules
# -- to do --
print "Creating scripts"
appman.create_script(config, self.host_name)
print "Starting modules on host: %s" % self.host_name
appman.run_script(self.host_name)
self.mod.SendSignal(rc.MT_APP_START_COMPLETE)
except Exception, e:
print "ERROR: %s" % (e)
elif msg_type == rc.MT_PING:
print 'got ping'
self.respond_to_ping(msg, 'AppStarter')
# we use this msg to stop modules individually
elif msg_type == MT_EXIT:
print 'got exit'
elif msg_type == MT_KILL:
print 'got kill'
appman.kill_modules()
class SimpleArbitrator(object):
debug = True
vel = np.zeros(rc.MAX_CONTROL_DIMS)
#pos = np.zeros(rc.MAX_CONTROL_DIMS)
autoVelControlFraction = \
np.ones_like(rc.MDF_ROBOT_CONTROL_CONFIG().autoVelControlFraction)
extrinsic_vel = np.zeros_like(rc.MDF_COMPOSITE_MOVEMENT_COMMAND().vel)
intrinsic_vel = np.zeros_like(rc.MDF_COMPOSITE_MOVEMENT_COMMAND().vel)
def __init__(self, config_file, server):
self.load_config(config_file)
self.setup_dragonfly(server)
self.run()
def load_config(self, config_file):
self.config = SafeConfigParser()
self.config.read(config_file)
self.timer_tag = self.config.get('main', 'timer_tag')
self.extrinsic_tags = self.config.get('main', 'extrinsic_tags').split()
self.intrinsic_tags = self.config.get('main', 'intrinsic_tags').split()
default_auto = float(self.config.get('main', 'default_auto'))
self.autoVelControlFraction[:] = default_auto
self.gate = 1. # default value
self.idle_gateable = 0. # default value
def setup_dragonfly(self, server):
self.mod = Dragonfly_Module(rc.MID_SIMPLE_ARBITRATOR, 0)
self.mod.ConnectToMMM(server)
self.mod.Subscribe(MT_EXIT)
for sub in subscriptions:
self.mod.Subscribe(eval('rc.MT_%s' % (sub)))
self.mod.SendModuleReady()
print "Connected to Dragonfly at", server
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.1)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'SimpleArbitrator')
else:
self.process_message(msg)
def process_message(self, msg):
'''
Needs to:
1) combine non-conflicting controlledDims e.g. from
OPERATOR_MOVEMENT_COMMANDs, into either extrinsic or
intrinsic commands
2) combine intrinsic and extrinsic commands into final command
'''
msg_type = msg.GetHeader().msg_type
if msg_type in [
rc.MT_OPERATOR_MOVEMENT_COMMAND,
rc.MT_PLANNER_MOVEMENT_COMMAND, rc.MT_EM_MOVEMENT_COMMAND,
rc.MT_FIXTURED_MOVEMENT_COMMAND
]:
if msg_type == rc.MT_OPERATOR_MOVEMENT_COMMAND:
mdf = rc.MDF_OPERATOR_MOVEMENT_COMMAND()
elif msg_type == rc.MT_PLANNER_MOVEMENT_COMMAND:
mdf = rc.MDF_PLANNER_MOVEMENT_COMMAND()
elif msg_type == rc.MT_EM_MOVEMENT_COMMAND:
mdf = rc.MDF_EM_MOVEMENT_COMMAND()
elif msg_type == rc.MT_FIXTURED_MOVEMENT_COMMAND:
mdf = rc.MDF_FIXTURED_MOVEMENT_COMMAND()
# MOVEMENT_COMMAND
# ----------------
# controlledDims
# pos
# sample_header
# sample_interval
# tag
# vel
# ----------------
copy_from_msg(mdf, msg)
tag = mdf.tag
#if not tag in self.accepted_tags:
# return
dim = np.asarray(mdf.controlledDims, dtype=bool) #.astype(bool)
if mdf.tag in self.intrinsic_tags:
# intrinsic is AUTO command
self.intrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim]
#print "intr_vel = " + " ".join(["%5.2f" % (x) for x in self.intrinsic_vel])
elif mdf.tag in self.extrinsic_tags:
#print "!"
# extrinsic is non-AUTO, i.e. EM, command
self.extrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim]
#self.extrinsic_vel[:8] *= self.gate
if tag == self.timer_tag:
self.send_output(mdf.sample_header)
elif msg_type == rc.MT_ROBOT_CONTROL_CONFIG:
mdf = rc.MDF_ROBOT_CONTROL_CONFIG()
copy_from_msg(mdf, msg)
self.autoVelControlFraction[:] = mdf.autoVelControlFraction
elif msg_type == rc.MT_IDLE:
mdf = rc.MDF_IDLE()
copy_from_msg(mdf, msg)
self.gate = float(np.asarray(mdf.gain, dtype=float).item())
elif msg_type == rc.MT_IDLE_DETECTION_ENDED:
self.gate = 1.0
elif msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, msg)
self.idle_gateable = mdf.idle_gateable
def get_combined_command(self):
C = 1 - self.autoVelControlFraction # extrinsic fraction
d = self.intrinsic_vel
u = self.extrinsic_vel
combined = C * u + (1 - C) * d
print "--------------------------------------"
print "C" + " ".join(["%0.2f" % (x) for x in C])
print "d" + " ".join(["%0.2f" % (x) for x in d])
print "u" + " ".join(["%0.2f" % (x) for x in u])
print "+" + " ".join(["%0.2f" % (x) for x in combined])
print "gain: ", self.gate
print "gateable: ", self.idle_gateable
return combined
def send_output(self, sample_header):
mdf = rc.MDF_COMPOSITE_MOVEMENT_COMMAND()
mdf.tag = 'composite'
vel = np.zeros_like(mdf.vel)
vel[:] = self.get_combined_command()
if self.idle_gateable == 1:
vel[:8] *= self.gate
mdf.vel[:] = vel
mdf.sample_header = sample_header
msg = CMessage(rc.MT_COMPOSITE_MOVEMENT_COMMAND)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
def update(self, dt):
while True:
rcv = self.mod.ReadMessage(self.msg, 0)
if rcv == 1:
hdr = self.msg.GetHeader()
msg_type = hdr.msg_type
if msg_type == rc.MT_PING:
self.reset_score()
elif msg_type == rc.MT_INPUT_DOF_DATA:
mdf = rc.MDF_INPUT_DOF_DATA()
copy_from_msg(mdf, self.msg)
if mdf.tag == 'carduinoIO':
fdbk = 5 - mdf.dof_vals[
7] # invert to match phyiscal setup
x_pos = int((fdbk * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
x_pos += 20
self.pos_fdbk_txt.text = "%.2f V" % fdbk
self.pos_fdbk.v[0] = (x_pos, 405)
self.pos_fdbk.v[1] = (x_pos, 615)
self.pos_fdbk.v[2] = (x_pos + 8, 615)
self.pos_fdbk.v[3] = (x_pos + 8, 405)
# if msg_type == rc.MT_FORCE_SENSOR_DATA:
# mdf = rc.MDF_FORCE_SENSOR_DATA()
# copy_from_msg(mdf, self.msg)
#
# x_fdbk = mdf.data[0]
# x_fdbk_width = int((x_fdbk / MAX_FDBK) * MAX_WIDTH)
elif msg_type == rc.MT_RT_POSITION_FEEDBACK: # updates real time position of handle on screen receives messages from cube_sphere while loop
mdf = rc.MDF_RT_POSITION_FEEDBACK()
copy_from_msg(mdf, self.msg)
x_pos = mdf.distanceFromCenter
x_pos += 20
self.pos_fdbk.v[0] = (x_pos, 405)
self.pos_fdbk.v[1] = (x_pos, 615)
self.pos_fdbk.v[2] = (x_pos + 8, 615)
self.pos_fdbk.v[3] = (x_pos + 8, 405)
self.resizePolygon(self.pos_fdbk)
self.transformPolygon(self.pos_fdbk,
self.transformationType)
elif msg_type == rc.MT_COMBO_WAIT:
mdf = rc.MDF_COMBO_WAIT()
copy_from_msg(mdf, self.msg)
print mdf.duration
duration = mdf.duration / 1000 # convert to seconds
self.timer_sec = duration % 60
self.timer_min = duration / 60
self.screen_off()
self.schedule_interval(self.timer_count_down, 1)
elif msg_type == rc.MT_TRIAL_CONFIG:
self.unschedule(self.timer_count_down)
self.combo_wait_txt.text = ''
self.screen_on()
elif msg_type == rc.MT_END_TASK_STATE:
mdf = rc.MDF_END_TASK_STATE()
#copy_from_msg(mdf, self.msg)
read_msg_data(mdf, self.msg)
print mdf.id, mdf.outcome
if (mdf.id == REWARD_TS) and (mdf.outcome == 1):
self.increment_score()
if (mdf.id in [2, 3, 4]) and (mdf.outcome == 0):
print "screen off"
self.screen_off()
elif msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, self.msg)
if mdf.background_color == 'gray':
self.color = (180, 180, 180)
elif mdf.background_color == 'red':
self.color = (150, 12, 12)
elif mdf.background_color == 'green':
self.color = (0, 150, 50)
if mdf.fdbk_display_color == 'gray':
self.tgt_window.color = (0.3, 0.3, 0.3, 1)
elif mdf.fdbk_display_color == 'yellow':
self.increment_score()
self.tgt_window.color = (0.5, 0.5, 0.0, 1)
elif mdf.fdbk_display_color == 'green':
self.tgt_window.color = (0.0, 0.6, 0.2, 1)
elif mdf.fdbk_display_color == 'red':
self.tgt_window.color = (0.6, 0.05, 0.05, 1)
if not math.isnan(
mdf.direction) and mdf.direction in range(
-1, 3
) and not mdf.direction == self.transformationType:
self.position_bar.v = [(20, 455), (20, 565),
(1260, 565), (1260, 455)]
self.resizePolygon(self.position_bar)
self.transformPolygon(self.position_bar, mdf.direction)
self.transformPolygon(self.pos_fdbk, mdf.direction)
self.transformationType = mdf.direction
if not (math.isnan(mdf.target[0])) and not (math.isnan(
mdf.target[1])):
x_tgt_lo = mdf.target[0] + 20
x_tgt_hi = mdf.target[1] + 20
self.tgt_window.v[0] = (x_tgt_lo, 430)
self.tgt_window.v[1] = (x_tgt_lo, 590)
self.tgt_window.v[2] = (x_tgt_hi, 590)
self.tgt_window.v[3] = (x_tgt_hi, 430)
self.resizePolygon(self.tgt_window)
self.transformPolygon(self.tgt_window,
self.transformationType)
else:
break
def process_msg(self, in_msg):
header = in_msg.GetHeader()
if header.msg_type == rc.MT_FT_DATA:
mdf = rc.MDF_FT_DATA()
copy_from_msg(mdf, in_msg)
rate(self.rate)
self.ball.pos = vector(mdf.F[0:3])
self.shadow_cursor.pos = vector(
[mdf.F[0], -self.length / 2, mdf.F[2]])
self.unit_target = np.array(self.target_vector) / np.linalg.norm(
self.target_vector)
self.target_position = np.array(
self.unit_target) * self.max_factor * self.force_scale
self.target.pos = self.target_position
self.shadow_target.pos = [
self.target_position[0], -self.length / 2,
self.target_position[2]
]
distance = [a - b for a, b in zip(self.ball.pos, self.target.pos)]
if (distance[0]**2 + distance[1]**2 + distance[2]**2)**(
1 / 2.) >= self.threshold and self.RTFT_display:
self.ball.color = self.ball_color
self.state = 0
elif (distance[0]**2 + distance[1]**2 + distance[2]**2)**(
1 / 2.) < self.threshold and self.RTFT_display:
if self.state == 0: # if previous sample was outside radius, and now we're inside...
self.start_hold = time.time()
self.state = 1
self.ball.color = color.orange
else:
if time.time() > (self.start_hold + self.hold_time):
self.ball.color = color.green
self.target.visible = False
self.shadow_target.visible = False
self.state = 2
out_mdf = rc.MDF_FT_COMPLETE()
out_mdf.FT_COMPLETE = self.state
out_mdf.sample_header = mdf.sample_header
msg = CMessage(rc.MT_FT_COMPLETE)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
else:
self.state = 1
self.ball.color = color.orange
else:
self.state = -1
if self.state == 2 and self.solo: #if no executive file
self.target.pos = [
float(x) for x in [
np.random.rand(1, 1) * self.max_factor *
self.force_scale,
np.random.rand(1, 1) * self.max_factor *
self.force_scale,
np.random.rand(1, 1) * self.max_factor *
self.force_scale
]
]
self.shadow_target.pos = [
self.target.pos[0], -self.length / 2, self.target.pos[2]
]
sys.stdout.write(
"%7.4f, %5d, %16.2f\n" %
(mdf.F[2], self.state,
(self.start_hold + self.hold_time) - time.time()))
#msg_str = "%7.4f " * 6 + "\n"
#sys.stdout.write(msg_str % (mdf.F[0], mdf.F[1], mdf.F[2],
# mdf.T[0], mdf.T[1], mdf.T[2]))
sys.stdout.flush()
elif header.msg_type == rc.MT_RTFT_CONFIG:
mdf = rc.MDF_RTFT_CONFIG()
copy_from_msg(mdf, in_msg)
self.max_factor = mdf.max_factor
self.RTFT_display = mdf.RTFT_display
self.target_vector = mdf.target_vector[:]
self.ball.visible = mdf.cursor_visible
self.target.visible = mdf.target_visible
self.shadow_target.visible = mdf.shadow_target_visible
self.shadow_cursor.visible = mdf.shadow_cursor_visible
self.ball_color = [1, 0, 0]
self.solo = False
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if not msg_type in self.msg_nums:
return
# SESSION_CONFIG => start of session
if msg_type == rc.MT_SESSION_CONFIG:
self.num_trials = 0
self.reset_counters()
# EM_DECODER_CONFIGURATION => end of an adaptation round
elif msg_type == rc.MT_EM_DECODER_CONFIGURATION:
self.reset_counters()
# END_TASK_STATE => end of a task
elif msg_type == rc.MT_END_TASK_STATE:
mdf = rc.MDF_END_TASK_STATE()
copy_from_msg(mdf, in_msg)
# need to know:
# begin task state code
# final task state code
# intertrial state code
if (mdf.id == 1):
self.trial_sync = 1
self.shadow_started_window.append(0)
if (mdf.id == self.task_state_codes['begin']) & (mdf.outcome == 1):
if self.trial_sync:
#print "*** trial started ***"
#self.rewards_given += 1
self.shadow_num_trial_started_postcalib += 1
self.shadow_success_window.append(0)
self.shadow_givenup_window.append(0)
self.shadow_started_window[-1] = 1
if mdf.reason == "JV_IDLE_TIMEOUT":
if self.trial_sync:
self.shadow_num_trial_givenup_postcalib += 1
self.shadow_givenup_window[-1] = 1
if (mdf.id == self.task_state_codes['final']) & (mdf.outcome == 1):
if self.trial_sync:
#print "*** trial complete and successful"
self.shadow_num_trial_successful_postcalib += 1
self.shadow_success_window[-1] = 1
if (mdf.id == self.task_state_codes['intertrial']):
if self.trial_sync:
# do end-of-trial stuff here
self.num_trials += 1
self.num_trials_postcalib += 1
self.num_trial_started_postcalib = self.shadow_num_trial_started_postcalib
self.num_trial_successful_postcalib = self.shadow_num_trial_successful_postcalib
self.num_trial_givenup_postcalib = self.shadow_num_trial_givenup_postcalib
if len(self.shadow_success_window) > self.window_len:
self.shadow_success_window.pop(0)
if len(self.shadow_givenup_window) > self.window_len:
self.shadow_givenup_window.pop(0)
if len(self.shadow_started_window) > self.window_len:
self.shadow_started_window.pop(0)
self.success_window = copy.deepcopy(
self.shadow_success_window)
self.started_window = copy.deepcopy(
self.shadow_started_window)
self.givenup_window = copy.deepcopy(
self.shadow_givenup_window)
def run(self):
while True:
in_msg = CMessage()
rcv = self.mod.ReadMessage(in_msg, 0.1)
if rcv == 1:
msg_type = in_msg.GetHeader().msg_type
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, in_msg, 'Metronome')
elif msg_type == rc.MT_MNOME_STATE:
print 'got message'
in_mdf = rc.MDF_MNOME_STATE()
copy_from_msg(in_mdf, in_msg)
if in_mdf.state == 0:
print 'got stop'
self.pause_state = True
self.count = 0
elif in_mdf.state == 1:
print 'got start'
self.pause_state = False
self.count = 0
elif in_mdf.state == 2:
print 'got pause'
self.pause_state = True
self.count = 0
elif msg_type == self.in_msg_num:
if self.pause_state:
pass
else:
self.count += 1
if self.pretrigger_time > 0:
if self.count == self.metronome_count:
in_mdf = eval('rc.MDF_%s()' %
(self.in_msg_type.upper()))
copy_from_msg(in_mdf, in_msg)
out_mdf = rc.MDF_TMS_TRIGGER()
out_mdf.sample_header = in_mdf.sample_header
out_msg = CMessage(rc.MT_TMS_TRIGGER)
copy_to_msg(out_mdf, out_msg)
self.mod.SendMessage(out_msg)
self.count = 0 - int(
np.random.uniform(0, 1.5, 1)[0] *
self.in_msg_freq)
if self.count == self.trigger_out_count:
sound_thread = threading.Thread(
target=self.play_sound)
sound_thread.start()
else:
if self.count == self.trigger_out_count:
in_mdf = eval('rc.MDF_%s()' %
(self.in_msg_type.upper()))
copy_from_msg(in_mdf, in_msg)
out_mdf = rc.MDF_TMS_TRIGGER()
out_mdf.sample_header = in_mdf.sample_header
out_msg = CMessage(rc.MT_TMS_TRIGGER)
copy_to_msg(out_mdf, out_msg)
self.mod.SendMessage(out_msg)
if self.count == self.metronome_count:
self.count = 0 - int(
np.random.uniform(0, 1.5, 1)[0] *
self.in_msg_freq)
sound_thread = threading.Thread(
target=self.play_sound)
sound_thread.start()
def timer_event(self):
done = False
while not done:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
# SESSION_CONFIG => start of session
if msg_type == rc.MT_SESSION_CONFIG:
#self.msg_cnt += 1
self.num_trials = 0
self.reset_counters()
self.update_gui_label_data()
# EM_DECODER_CONFIGURATION => end of an adaptation round
elif msg_type == rc.MT_EM_DECODER_CONFIGURATION:
#self.msg_cnt += 1
self.reset_counters()
self.update_gui_label_data()
# END_TASK_STATE => end of a task
elif msg_type == rc.MT_END_TASK_STATE:
#self.msg_cnt += 1
mdf = rc.MDF_END_TASK_STATE()
copy_from_msg(mdf, msg)
# need to know:
# begin task state code
# final task state code
# intertrial state code
if (mdf.id == 1):
self.trial_sync = 1
self.shadow_started_window.append(0)
if (mdf.id == self.task_state_codes['begin']) & (mdf.outcome == 1):
if self.trial_sync:
#print "*** trial started ***"
#self.rewards_given += 1
self.shadow_num_trial_started_postcalib += 1
self.shadow_success_window.append(0)
self.shadow_givenup_window.append(0)
self.shadow_started_window[-1] = 1
if mdf.reason == "JV_IDLE_TIMEOUT":
if self.trial_sync:
self.shadow_num_trial_givenup_postcalib += 1
self.shadow_givenup_window[-1] = 1
if (mdf.id == self.task_state_codes['final']) & (mdf.outcome == 1):
if self.trial_sync:
#print "*** trial complete and successful"
self.shadow_num_trial_successful_postcalib += 1
self.shadow_success_window[-1] = 1
if (mdf.id == self.task_state_codes['intertrial']):
if self.trial_sync:
# do end-of-trial stuff here
self.num_trials += 1
self.num_trials_postcalib += 1
self.num_trial_started_postcalib = self.shadow_num_trial_started_postcalib
self.num_trial_successful_postcalib = self.shadow_num_trial_successful_postcalib
self.num_trial_givenup_postcalib = self.shadow_num_trial_givenup_postcalib
if len(self.shadow_success_window) > self.window_wide: #self.window_narrow:
self.shadow_success_window.pop(0)
if len(self.shadow_givenup_window) > self.window_wide: #self.window_narrow:
self.shadow_givenup_window.pop(0)
if len(self.shadow_started_window) > self.window_wide: #self.window_narrow:
self.shadow_started_window.pop(0)
self.success_window = copy.deepcopy(self.shadow_success_window)
self.started_window = copy.deepcopy(self.shadow_started_window)
self.givenup_window = copy.deepcopy(self.shadow_givenup_window)
if self.num_trials_postcalib > 0:
self.percent_start = 100 * self.num_trial_started_postcalib / self.num_trials_postcalib
self.percent_givenup = 100 * self.num_trial_givenup_postcalib / self.num_trials_postcalib
self.percent_success = 100 * self.num_trial_successful_postcalib / self.num_trials_postcalib
percent_success_wide_window = np.NAN
if len(self.success_window) >= self.window_wide:
num_success_window = np.sum(self.success_window)
percent_success_wide_window = 100 * num_success_window / len(self.success_window)
percent_givenup_wide_window = np.NAN
if len(self.givenup_window) >= self.window_wide:
num_givenup_window = np.sum(self.givenup_window)
percent_givenup_wide_window = 100 * num_givenup_window / len(self.givenup_window)
percent_started_wide_window = np.NAN
if len(self.started_window) >= self.window_wide:
num_started_window = np.sum(self.started_window)
percent_started_wide_window = 100 * num_started_window / len(self.started_window)
percent_success_narrow_window = np.NAN
if len(self.success_window) >= self.window_narrow:
success_window_narrow = self.success_window[len(self.success_window)-self.window_narrow:]
num_success_window = np.sum(success_window_narrow)
percent_success_narrow_window = 100 * num_success_window / len(success_window_narrow)
percent_givenup_narrow_window = np.NAN
if len(self.givenup_window) >= self.window_narrow:
givenup_window_narrow = self.givenup_window[len(self.givenup_window)-self.window_narrow:]
num_givenup_window = np.sum(givenup_window_narrow)
percent_givenup_narrow_window = 100 * num_givenup_window / len(givenup_window_narrow)
if len(self.started_window) >= self.window_narrow:
started_window_narrow = self.started_window[len(self.started_window)-self.window_narrow:]
num_started_window = np.sum(started_window_narrow)
percent_started_narrow_window = 100 * num_started_window / len(started_window_narrow)
self.hist_narrow_STR.append(percent_started_narrow_window)
self.hist_narrow_SUR.append(percent_success_narrow_window)
self.hist_narrow_GUR.append(percent_givenup_narrow_window)
self.hist_wide_STR.append(percent_started_wide_window)
self.hist_wide_SUR.append(percent_success_wide_window)
self.hist_wide_GUR.append(percent_givenup_wide_window)
self.update_gui_label_data()
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'TrialStatusDisplay')
elif msg_type == MT_EXIT:
self.exit()
done = True
else:
done = True
self.console_disp_cnt += 1
if self.console_disp_cnt == 50:
self.update_plot()
self.console_disp_cnt = 0
def process_message(self, msg):
'''
Needs to:
1) combine non-conflicting controlledDims e.g. from
OPERATOR_MOVEMENT_COMMANDs, into either extrinsic or
intrinsic commands
2) combine intrinsic and extrinsic commands into final command
'''
msg_type = msg.GetHeader().msg_type
if msg_type in [
rc.MT_OPERATOR_MOVEMENT_COMMAND,
rc.MT_PLANNER_MOVEMENT_COMMAND, rc.MT_EM_MOVEMENT_COMMAND,
rc.MT_FIXTURED_MOVEMENT_COMMAND
]:
if msg_type == rc.MT_OPERATOR_MOVEMENT_COMMAND:
mdf = rc.MDF_OPERATOR_MOVEMENT_COMMAND()
elif msg_type == rc.MT_PLANNER_MOVEMENT_COMMAND:
mdf = rc.MDF_PLANNER_MOVEMENT_COMMAND()
elif msg_type == rc.MT_EM_MOVEMENT_COMMAND:
mdf = rc.MDF_EM_MOVEMENT_COMMAND()
elif msg_type == rc.MT_FIXTURED_MOVEMENT_COMMAND:
mdf = rc.MDF_FIXTURED_MOVEMENT_COMMAND()
# MOVEMENT_COMMAND
# ----------------
# controlledDims
# pos
# sample_header
# sample_interval
# tag
# vel
# ----------------
copy_from_msg(mdf, msg)
tag = mdf.tag
#if not tag in self.accepted_tags:
# return
dim = np.asarray(mdf.controlledDims, dtype=bool) #.astype(bool)
if mdf.tag in self.intrinsic_tags:
# intrinsic is AUTO command
self.intrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim]
#print "intr_vel = " + " ".join(["%5.2f" % (x) for x in self.intrinsic_vel])
elif mdf.tag in self.extrinsic_tags:
#print "!"
# extrinsic is non-AUTO, i.e. EM, command
self.extrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim]
#self.extrinsic_vel[:8] *= self.gate
if tag == self.timer_tag:
self.send_output(mdf.sample_header)
elif msg_type == rc.MT_ROBOT_CONTROL_CONFIG:
mdf = rc.MDF_ROBOT_CONTROL_CONFIG()
copy_from_msg(mdf, msg)
self.autoVelControlFraction[:] = mdf.autoVelControlFraction
elif msg_type == rc.MT_IDLE:
mdf = rc.MDF_IDLE()
copy_from_msg(mdf, msg)
self.gate = float(np.asarray(mdf.gain, dtype=float).item())
elif msg_type == rc.MT_IDLE_DETECTION_ENDED:
self.gate = 1.0
elif msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, msg)
self.idle_gateable = mdf.idle_gateable
def update(self, dt):
while True:
rcv = self.mod.ReadMessage(self.msg, 0)
if rcv == 1:
hdr = self.msg.GetHeader()
msg_type = hdr.msg_type
if msg_type == rc.MT_PING:
self.reset_score()
elif msg_type == rc.MT_INPUT_DOF_DATA:
mdf = rc.MDF_INPUT_DOF_DATA()
copy_from_msg(mdf, self.msg)
if mdf.tag == 'carduinoIO':
fdbk = 5 - mdf.dof_vals[
7] # invert to match phyiscal setup
x_pos = int((fdbk * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
self.pos_fdbk_txt.text = "%.2f V" % fdbk
self.pos_fdbk.v[0] = (x_pos, 405)
self.pos_fdbk.v[1] = (x_pos, 615)
self.pos_fdbk.v[2] = (x_pos + 8, 615)
self.pos_fdbk.v[3] = (x_pos + 8, 405)
# if msg_type == rc.MT_FORCE_SENSOR_DATA:
# mdf = rc.MDF_FORCE_SENSOR_DATA()
# copy_from_msg(mdf, self.msg)
#
# x_fdbk = mdf.data[0]
# x_fdbk_width = int((x_fdbk / MAX_FDBK) * MAX_WIDTH)
elif msg_type == rc.MT_COMBO_WAIT:
mdf = rc.MDF_COMBO_WAIT()
copy_from_msg(mdf, self.msg)
print mdf.duration
duration = mdf.duration / 1000 # convert to seconds
self.timer_sec = duration % 60
self.timer_min = duration / 60
self.schedule_interval(self.timer_count_down, 1)
elif msg_type == rc.MT_TRIAL_CONFIG:
self.unschedule(self.timer_count_down)
self.combo_wait_txt.text = ''
# start displaying again
self.blank_display = False
self.color = (180, 180, 180)
elif msg_type == rc.MT_END_TASK_STATE:
mdf = rc.MDF_END_TASK_STATE()
copy_from_msg(mdf, self.msg)
if (mdf.id == REWARD_TS) and (mdf.outcome == 1):
self.increment_score()
if (mdf.id in [2, 3, 4]) and (mdf.outcome == 0):
self.color = (0, 0, 0)
self.blank_display = True
elif msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, self.msg)
if mdf.background_color == 'gray':
self.color = (180, 180, 180)
elif mdf.background_color == 'red':
self.color = (150, 12, 12)
elif mdf.background_color == 'green':
self.color = (0, 150, 50)
if mdf.fdbk_display_color == 'gray':
self.tgt_window.color = (0.3, 0.3, 0.3, 1)
elif mdf.fdbk_display_color == 'yellow':
self.tgt_window.color = (0.5, 0.5, 0.0, 1)
elif mdf.fdbk_display_color == 'green':
self.tgt_window.color = (0.0, 0.6, 0.2, 1)
elif mdf.fdbk_display_color == 'red':
self.tgt_window.color = (0.6, 0.05, 0.05, 1)
if not (math.isnan(mdf.target[0])) and not (math.isnan(
mdf.target[1])):
x_tgt_lo = int(
(mdf.target[0] * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
x_tgt_hi = int(
(mdf.target[1] * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
self.tgt_window.v[0] = (x_tgt_lo, 430)
self.tgt_window.v[1] = (x_tgt_lo, 590)
self.tgt_window.v[2] = (x_tgt_hi, 590)
self.tgt_window.v[3] = (x_tgt_hi, 430)
# update multipliers during "ForceRamp" task state
if mdf.id == FORCERAMP_TS:
force_level = mdf.sep_threshold_f[1]
if force_level > self.score_force_level:
self.score_force_level = force_level
self.score_force_mult += 1
# new combo, reset target multipliers
self.score_target_mult = 0
self.score_target_dist = 999
target_level = mdf.target[1] - mdf.target[0]
if target_level < self.score_target_dist:
self.score_target_dist = target_level
self.score_target_mult += 1
self.update_reward()
#print mdf.id
#print force_level, self.score_force_level, self.score_force_mult
#print target_level, self.score_target_dist, self.score_target_mult
#print "\n"
else:
break
def find_hotspot_to_cz(
self, HS_GCS, glasses_orientation, glasses_position,
coil_vector): #Saves 0, 0, 0 array to file when object called
check_HS_GCS = self.check(HS_GCS)
check_glasses_orientation = self.check(glasses_orientation)
check_glasses_position = self.check(glasses_position)
check_coil_vector = self.check(coil_vector)
if np.any(
np.array([
check_HS_GCS, check_glasses_orientation,
check_glasses_position, check_coil_vector
])):
print np.array([
check_HS_GCS, check_glasses_orientation,
check_glasses_position, check_coil_vector
])
HS_LCS = (np.zeros(3) * np.NaN)[None]
print HS_LCS
vector_LCS = (np.zeros(3) * np.NaN)[None]
with open(self.location, 'a') as location:
np.savetxt(location,
np.array(HS_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
location.close()
with open(self.vector, 'a') as data:
np.savetxt(data,
np.array(vector_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
data.close()
sys.stdout.write("No position\n")
else:
cz_pos, cz_rot = self.glasses_to_cz.get_pos(
glasses_orientation, glasses_position)
LT_pos, LT_rot = self.glasses_to_LT.get_pos(
glasses_orientation, glasses_position)
RT_pos, RT_rot = self.glasses_to_RT.get_pos(
glasses_orientation, glasses_position)
Nasion_pos, Nasion_rot = self.glasses_to_Nasion.get_pos(
glasses_orientation, glasses_position)
calibration_matrix = self.make_head_axis(LT_pos, RT_pos,
Nasion_pos)
HS_LCS = qa.rotate(calibration_matrix, (HS_GCS - cz_pos))
vector_LCS = qa.rotate(calibration_matrix, coil_vector)
#HS_LCS = qa.rotate(rot, qa.rotate(self.calibration_matrix, (HS_GCS - cz_pos)))
#vector_LCS = qa.rotate(rot, qa.rotate(self.calibration_matrix, (coil_vector)))
with open(self.location, 'a') as location:
np.savetxt(location,
np.array(HS_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
location.close()
with open(self.vector, 'a') as data:
np.savetxt(data,
np.array(vector_LCS),
fmt='%f',
delimiter=',',
newline='\r\n')
data.close()
self.stimulus_no += 1
print self.stimulus_no
in_mdf = rc.MDF_POLARIS_POSITION()
out_mdf = rc.MDF_PLOT_POSITION()
out_mdf.xyz[:] = HS_LCS[0]
out_mdf.ori[:] = np.append(vector_LCS[0],
0) # Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")