def getProgress(self, emgimu_l, emgimu_u):
"""Tracking the progress"""
emg_l = preprocess.process_emg(np.array(emgimu_l.emg), self.EMG_MAX,
self.EMG_MIN)
acc_l = np.array(emgimu_l.linear_acceleration)
gyro_l = preprocess.process_gyro(np.array(emgimu_l.angular_velocity),
self.GYRO_MAX,
discrete=False)
orie_l = np.array(emgimu_l.orientation)
emg_u = preprocess.process_emg(np.array(emgimu_u.emg), self.EMG_MAX,
self.EMG_MIN)
acc_u = np.array(emgimu_u.linear_acceleration)
gyro_u = preprocess.process_gyro(np.array(emgimu_u.angular_velocity),
self.GYRO_MAX,
discrete=False)
orie_u = np.array(emgimu_u.orientation)
signal_array = np.hstack((EMG_WEIGHT * emg_l, acc_l, gyro_l, orie_l,
EMG_WEIGHT * emg_u, acc_u, gyro_u, orie_u))
self.history.append(signal_array)
if len(self.previous) > 10: # 0.2 seconds
self.previous.pop(0)
self.previous.append(signal_array)
current_signal = np.mean(self.previous, axis=0)
# "state" here means a critical point, the output of the signal classifer
state = int(self.classifier.predict(current_signal)[0])
self.state_tracer.append(state)
self.pub1.publish(str(state))
if len(self.state_tracer) < 5:
return
if state != -1 and state == self.state_tracer[
-2] and state == self.state_tracer[-3]:
if not self.state_history:
self.state_history.append(state)
elif state != self.state_history[-1]:
self.state_history.append(state)
print "state history: ", self.state_history
self.current_segment.append(len(self.history))
self.progress = self.getPosition(state) * 1.0 / (self.n_states - 1)
if state in self.task:
self.task.remove(state)
if state != self.recent_state:
self.delay = 0
self.recent_state = state
self.delay += 1
if len(self.task) > 0 and self.delay > 500:
#self.deliver_prompt()
print "stuck"
def getProgress(self, emgimu_l, emgimu_u):
#print "Tracking your progress now..."
emg_l = preprocess.process_emg(np.array(emgimu_l.emg), self.EMG_MAX,
self.EMG_MIN)
acc_l = np.array(emgimu_l.linear_acceleration)
gyro_l = preprocess.process_gyro(np.array(emgimu_l.angular_velocity),
self.GYRO_MAX,
discrete=False)
orie_l = np.array(emgimu_l.orientation)
emg_u = preprocess.process_emg(np.array(emgimu_u.emg), self.EMG_MAX,
self.EMG_MIN)
acc_u = np.array(emgimu_u.linear_acceleration)
gyro_u = preprocess.process_gyro(np.array(emgimu_u.angular_velocity),
self.GYRO_MAX,
discrete=False)
orie_u = np.array(emgimu_u.orientation)
signal_array = np.hstack((EMG_WEIGHT * emg_l, acc_l, gyro_l, orie_l,
EMG_WEIGHT * emg_u, acc_u, gyro_u, orie_u))
self.history.append(signal_array)
# print "# data points saved: ", len(self.history)
if len(self.previous) > 10: # 0.2 seconds
self.previous.pop(0)
self.previous.append(signal_array)
current_signal = np.mean(self.previous, axis=0)
state = int(self.classifier.predict(current_signal)[0])
self.state_tracer.append(state)
self.pub1.publish(str(state))
if len(self.state_tracer) < 5:
return
if state != -1 and state == self.state_tracer[
-2] and state == self.state_tracer[-3]:
if not self.state_history:
self.state_history.append(state)
elif state != self.state_history[-1]:
self.state_history.append(state)
print "state history: ", self.state_history
self.current_segment.append(len(self.history))
self.progress = self.getPosition(state) * 1.0 / (self.n_states - 1)
#print "progress: ", self.progress
# print "current state", state, state_map[state]
if state in self.task:
self.task.remove(state)
#self.progress = 1 - 1.0*len(self.task)/(self.n_states-1)
if state != self.recent_state:
self.delay = 0
self.recent_state = state
self.delay += 1
def getProgress(self, emgimu_l, emgimu_u):
print "Tracking your progress now..."
emg_l = preprocess.process_emg(np.array(emgimu_l.emg), self.EMG_MAX_l, self.EMG_MIN_l)
acc_l = np.array(emgimu_l.linear_acceleration)
gyro_l = preprocess.process_gyro(np.array(emgimu_l.angular_velocity), self.GYRO_MAX_l, discrete=False)
orie_l = np.array(emgimu_l.orientation)
emg_u = preprocess.process_emg(np.array(emgimu_u.emg), self.EMG_MAX_u, self.EMG_MIN_u)
acc_u = np.array(emgimu_u.linear_acceleration)
gyro_u = preprocess.process_gyro(np.array(emgimu_u.angular_velocity), self.GYRO_MAX_u, discrete=False)
orie_u = np.array(emgimu_u.orientation)
signal_array = np.hstack((EMG_WEIGHT*emg_l, acc_l, gyro_l, orie_l, EMG_WEIGHT*emg_u, acc_u, gyro_u, orie_u))
self.history.append(signal_array)
if len(self.previous)>10: # 0.2 seconds
self.previous.pop(0)
self.previous.append(signal_array)
current_signal = np.mean(self.previous, axis=0)
state = 's'+str(int(self.classifier.predict(current_signal)[0]))
self.pub1.publish(state)
self.pub.publish(self.progress)
if self.task == []:
print "Task completed!"
self.pub.publish(1.0)
self.task = self.full_task[:]
self.start = False
self.progress = 0
self.evaluate_pfmce()
#sys.exit()
if state == self.task[0]:
self.count_down -= 1
if self.count_down == 0:
# print "current state", state, state_map[state]
self.task.pop(0)
self.progress = 1 - 1.0*len(self.task)/(self.n_states-1)
self.pub.publish(self.progress)
self.count_down = 10
self.delay = 0
elif self.give_prompt:
# no transition
self.delay += 1
if len(self.task)>0 and self.delay>500:
#self.start = False
self.prompt_now = True
print "prompt started"
print self.prompt_now
self.prompt.callback(self.progress)
print "prompt ended"
self.delay = 0
self.prompt_now = False
def getProgress(self, emgimu_l, emgimu_u):
"""Tracking the progress"""
emg_l = preprocess.process_emg(np.array(emgimu_l.emg), self.EMG_MAX, self.EMG_MIN)
acc_l = np.array(emgimu_l.linear_acceleration)
gyro_l = preprocess.process_gyro(np.array(emgimu_l.angular_velocity), self.GYRO_MAX, discrete=False)
orie_l = np.array(emgimu_l.orientation)
emg_u = preprocess.process_emg(np.array(emgimu_u.emg), self.EMG_MAX, self.EMG_MIN)
acc_u = np.array(emgimu_u.linear_acceleration)
gyro_u = preprocess.process_gyro(np.array(emgimu_u.angular_velocity), self.GYRO_MAX, discrete=False)
orie_u = np.array(emgimu_u.orientation)
signal_array = np.hstack((EMG_WEIGHT*emg_l, acc_l, gyro_l, orie_l, EMG_WEIGHT*emg_u, acc_u, gyro_u, orie_u))
self.history.append(signal_array)
if len(self.previous)>10: # 0.2 seconds
self.previous.pop(0)
self.previous.append(signal_array)
current_signal = np.mean(self.previous, axis=0)
# "state" here means a critical point, the output of the signal classifer
state = int(self.classifier.predict(current_signal)[0])
self.state_tracer.append(state)
self.pub1.publish(str(state))
if len(self.state_tracer) < 5:
return
if state != -1 and state == self.state_tracer[-2] and state == self.state_tracer[-3]:
if not self.state_history:
self.state_history.append(state)
elif state != self.state_history[-1]:
self.state_history.append(state)
print "state history: ", self.state_history
self.current_segment.append(len(self.history))
self.progress = self.getPosition(state) * 1.0 / (self.n_states - 1)
if state in self.task:
self.task.remove(state)
if state != self.recent_state:
self.delay = 0
self.recent_state = state
self.delay += 1
if len(self.task)>0 and self.delay>500:
#self.deliver_prompt()
print "stuck"
def getProgress(self, emgimu_l, emgimu_u):
#print "Tracking your progress now..."
emg_l = preprocess.process_emg(np.array(emgimu_l.emg), self.EMG_MAX, self.EMG_MIN)
acc_l = np.array(emgimu_l.linear_acceleration)
gyro_l = preprocess.process_gyro(np.array(emgimu_l.angular_velocity), self.GYRO_MAX, discrete=False)
orie_l = np.array(emgimu_l.orientation)
emg_u = preprocess.process_emg(np.array(emgimu_u.emg), self.EMG_MAX, self.EMG_MIN)
acc_u = np.array(emgimu_u.linear_acceleration)
gyro_u = preprocess.process_gyro(np.array(emgimu_u.angular_velocity), self.GYRO_MAX, discrete=False)
orie_u = np.array(emgimu_u.orientation)
signal_array = np.hstack((EMG_WEIGHT*emg_l, acc_l, gyro_l, orie_l, EMG_WEIGHT*emg_u, acc_u, gyro_u, orie_u))
self.history.append(signal_array)
# print "# data points saved: ", len(self.history)
if len(self.previous)>10: # 0.2 seconds
self.previous.pop(0)
self.previous.append(signal_array)
current_signal = np.mean(self.previous, axis=0)
state = int(self.classifier.predict(current_signal)[0])
self.state_tracer.append(state)
self.pub1.publish(str(state))
if len(self.state_tracer) < 5:
return
if state != -1 and state == self.state_tracer[-2] and state == self.state_tracer[-3]:
if not self.state_history:
self.state_history.append(state)
elif state != self.state_history[-1]:
self.state_history.append(state)
print "state history: ", self.state_history
self.current_segment.append(len(self.history))
self.progress = self.getPosition(state) * 1.0 / (self.n_states - 1)
#print "progress: ", self.progress
# print "current state", state, state_map[state]
if state in self.task:
self.task.remove(state)
#self.progress = 1 - 1.0*len(self.task)/(self.n_states-1)
if state != self.recent_state:
self.delay = 0
self.recent_state = state
self.delay += 1
def getProgress(self, emgimu):
#=======================================================================
# if self.baseRot is None:
# self.baseRot = imu.orientation.x * pi/180 # used for calibration
#=======================================================================
#quat = tf.transformations.quaternion_from_euler(-imu.orientation.z, imu.orientation.y, -imu.orientation.x + self.baseRot)
#===============================================================================
# imu_array = np.array([imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z,
# imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z,
# imu.orientation.x, imu.orientation.y,imu.orientation.z, imu.orientation.w])
# # print imu_array
# #imu_array[3:6] = imu_array[3:6]/500
# imu_array[3:6] = process_gyro(imu_array[3:6])
#===============================================================================
emg = preprocess.process_emg(np.array(emgimu.emg), self.EMG_MAX,
self.EMG_MIN)
acc = np.array(emgimu.linear_acceleration)
gyro = preprocess.process_gyro(np.array(emgimu.angular_velocity),
self.GYRO_MAX,
discrete=False)
orie = np.array(emgimu.orientation)
signal_array = np.hstack((EMG_WEIGHT * emg, acc, gyro, orie))
self.history.append(signal_array)
if len(self.previous) > 10: # 0.2 seconds
self.previous.pop(0)
self.previous.append(signal_array)
current_signal = np.mean(self.previous, axis=0)
#imu_array = np.array([imu.x, imu.y, imu.z, imu.w])
#print current_signal
#state = 's'+str(int(self.classifier.predict(imu_array)[0]))
state = 's' + str(int(self.classifier.predict(current_signal)[0]))
self.pub1.publish(state)
self.pub.publish(self.progress)
if self.task == []:
print "Task completed!"
self.pub.publish(1.0)
self.task = self.full_task[:]
self.start = False
self.progress = 0
self.evaluate_pfmce()
#sys.exit()
if state == self.task[0]:
self.count_down -= 1
if self.count_down == 0:
# print "current state", state, state_map[state]
self.task.pop(0)
self.progress = 1 - 1.0 * len(self.task) / (self.n_states - 1)
self.pub.publish(self.progress)
self.count_down = 10
self.delay = 0
elif self.give_prompt:
# no transition
self.delay += 1
if len(self.task) > 0 and self.delay > 500:
#self.start = False
self.prompt_now = True
print "prompt started"
print self.prompt_now
self.prompt.callback(self.progress)
print "prompt ended"
self.delay = 0
self.prompt_now = False
def getProgress(self, emgimu):
#=======================================================================
# if self.baseRot is None:
# self.baseRot = imu.orientation.x * pi/180 # used for calibration
#=======================================================================
#quat = tf.transformations.quaternion_from_euler(-imu.orientation.z, imu.orientation.y, -imu.orientation.x + self.baseRot)
#===============================================================================
# imu_array = np.array([imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z,
# imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z,
# imu.orientation.x, imu.orientation.y,imu.orientation.z, imu.orientation.w])
# # print imu_array
# #imu_array[3:6] = imu_array[3:6]/500
# imu_array[3:6] = process_gyro(imu_array[3:6])
#===============================================================================
emg = preprocess.process_emg(np.array(emgimu.emg), self.EMG_MAX, self.EMG_MIN)
acc = np.array(emgimu.linear_acceleration)
gyro = preprocess.process_gyro(np.array(emgimu.angular_velocity), self.GYRO_MAX, discrete=False)
orie = np.array(emgimu.orientation)
signal_array = np.hstack((EMG_WEIGHT*emg, acc, gyro, orie))
self.history.append(signal_array)
if len(self.previous)>10: # 0.2 seconds
self.previous.pop(0)
self.previous.append(signal_array)
current_signal = np.mean(self.previous, axis=0)
#imu_array = np.array([imu.x, imu.y, imu.z, imu.w])
#print current_signal
#state = 's'+str(int(self.classifier.predict(imu_array)[0]))
state = 's'+str(int(self.classifier.predict(current_signal)[0]))
self.pub1.publish(state)
self.pub.publish(self.progress)
if self.task == []:
print "Task completed!"
self.pub.publish(1.0)
self.task = self.full_task[:]
self.start = False
self.progress = 0
self.evaluate_pfmce()
#sys.exit()
if state == self.task[0]:
self.count_down -= 1
if self.count_down == 0:
# print "current state", state, state_map[state]
self.task.pop(0)
self.progress = 1 - 1.0*len(self.task)/(self.n_states-1)
self.pub.publish(self.progress)
self.count_down = 10
self.delay = 0
elif self.give_prompt:
# no transition
self.delay += 1
if len(self.task)>0 and self.delay>500:
#self.start = False
self.prompt_now = True
print "prompt started"
print self.prompt_now
self.prompt.callback(self.progress)
print "prompt ended"
self.delay = 0
self.prompt_now = False