示例#1
0
    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"
示例#2
0
    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
示例#3
0
    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
示例#4
0
    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"
示例#5
0
    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
示例#6
0
    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
示例#7
0
    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