def hand_gain(self, factor): # Increase the speed of the hand and apply max / min constraints self.hand_gain_value *= factor if self.hand_gain_value < get_user_config_var('MPL.HandSpeedMin', 0.1): self.hand_gain_value = get_user_config_var('MPL.HandSpeedMin', 0.1) if self.hand_gain_value > get_user_config_var('MPL.HandSpeedMax', 5): self.hand_gain_value = get_user_config_var('MPL.HandSpeedMax', 5)
def get(self): from os import path homepath = get_user_config_var('MobileApp.path', "../www/mplHome") homepage = get_user_config_var('MobileApp.homepage', "index.html") homepage_path = path.join(homepath, homepage) loader = tornado.template.Loader(".") self.write(loader.load(homepage_path).generate())
def __init__(self, dt, roc_filename): # store current position and velocity commands self.joint_position = np.zeros(MplId.NUM_JOINTS) self.joint_velocity = np.zeros(MplId.NUM_JOINTS) # Load limits from xml config file self.lower_limit = np.zeros(MplId.NUM_JOINTS) self.upper_limit = np.zeros(MplId.NUM_JOINTS) # Basic time step for integration self.dt = dt # currently selected ROC for arm motion self.roc_id = '' self.roc_position = 0.0 self.roc_velocity = 0.0 # currently selected ROC for hand motion self.grasp_id = '' self.grasp_position = 0.0 self.grasp_velocity = 0.0 self.roc_filename = roc_filename self.roc_table = None self.load_roc() self.load_config_parameters() # for residual limb arm tracking self.myo_position_1 = user_config.get_user_config_var('myo_position_1', 'AE') self.myo_position_2 = user_config.get_user_config_var('myo_position_2', 'AE') self.arm_side = user_config.get_user_config_var('MotionTrack.arm_side', 'right') self.ref_frame_upper = np.eye(4) # offset self.ref_frame_lower = np.eye(4)
def setup_threads(): # get parameters from xml files and create Servers s1 = MyoUdpServer(name='Myo1') s1.iface = uc.get_user_config_var("MyoUdpServer.iface_1", 0) s1.mac_address = uc.get_user_config_var("MyoUdpServer.mac_address_1", 'XX:XX:XX:XX:XX:XX') local_port_str = uc.get_user_config_var("MyoUdpServer.local_address_1", '//0.0.0.0:16001') s1.local_port = get_address(local_port_str) remote_port_str = uc.get_user_config_var("MyoUdpServer.remote_address_1", '//127.0.0.1:15001') s1.remote_port = get_address(remote_port_str) s1.setup_logger() s1.setup_devices() if uc.get_user_config_var('MyoUdpServer.num_devices', 2) < 2: s2 = None return s1, s2 s2 = MyoUdpServer(name='Myo2') s2.iface = uc.get_user_config_var("MyoUdpServer.iface_2", 0) s2.mac_address = uc.get_user_config_var("MyoUdpServer.mac_address_2", 'XX:XX:XX:XX:XX:XX') local_port_str = uc.get_user_config_var("MyoUdpServer.local_address_2", '//0.0.0.0:16001') s2.local_port = get_address(local_port_str) remote_port_str = uc.get_user_config_var("MyoUdpServer.remote_address_2", '//127.0.0.1:15001') s2.remote_port = get_address(remote_port_str) s2.setup_logger() s2.setup_devices() return s1, s2
def __init__(self): import threading # Initialize superclass super(AppInterface, self).__init__() homepath = get_user_config_var('MobileApp.path', "../www/mplHome") # handle to websocket interface self.application = tornado.web.Application([ (r'/ws', WSHandler), (r'/', MainHandler), (r'/test_area', TestHandler), (r"/(.*)", tornado.web.StaticFileHandler, { "path": homepath }), ]) self.last_msg = message_history # keep count of skipped messages so we can send at some nominal rate self.msg_skip_count = 0 self.thread = threading.Thread( target=tornado.ioloop.IOLoop.instance().start, name='WebThread')
def load_config_parameters(self): # Load parameters from xml config file self.joint_offset = [0.0] * MplId.NUM_JOINTS for i in range(MplId.NUM_JOINTS): self.joint_offset[i] = np.deg2rad( get_user_config_var(MplId(i).name + '_OFFSET', 0.0))
def setup_load_cell(): from inputs import dcell # Setup Additional Logging if get_user_config_var('DCell.enable', 0): # Start DCell Streaming port = get_user_config_var('DCell.serial_port', '/dev/ttymxc2') dc = dcell.DCellSerial(port) # Connect and start streaming dc.enable_data_logging = True try: dc.connect() logging.info('DCell streaming started successfully') except Exception: log = logging.getLogger() log.exception('Error from DCELL:')
def __init__(self): # store the last known limb position; None indicates no valid percepts received self.position = { 'last_percept': None, 'home': [0.0] * Mpl.NUM_JOINTS, 'park': [0.0] * Mpl.NUM_JOINTS } for i in range(Mpl.NUM_JOINTS): self.position['park'][i] = np.deg2rad( uc.get_user_config_var(Mpl(i).name + '_POS_PARK', 0.0)) for i in range(Mpl.NUM_JOINTS): self.position['home'][i] = np.deg2rad( uc.get_user_config_var(Mpl(i).name + '_POS_HOME', 0.0))
def setup_interfaces(self): from interface import assessment, app_services from inputs import normalization # Setup web/websocket interface server_type = get_user_config_var('MobileApp.server_type', 'None') if server_type == 'Tornado': port = get_user_config_var('MobileApp.port', 9090) self.TrainingInterface = app_services.TrainingManagerWebsocket() self.TrainingInterface.setup(port) print(f'Started webserver at http://localhost:{port}') # Assign modules to the app for assessments and rotational corrections if self.TrainingInterface is not None: # Setup Assessments tac = assessment.TargetAchievementControl(self, self.TrainingInterface) motion_test = assessment.MotionTester(self, self.TrainingInterface) myo_norm = normalization.MyoNormalization(self, self.TrainingInterface) # assign message callbacks self.TrainingInterface.add_message_handler(self.command_string) self.TrainingInterface.add_message_handler(tac.command_string) self.TrainingInterface.add_message_handler(motion_test.command_string) self.TrainingInterface.add_message_handler(myo_norm.command_string)
def main(args): # setup logging. This will create a log file like: USER_2016-02-11_11-28-21.log to which all 'logging' calls go uc.read_user_config_file(file=args.XML) uc.setup_file_logging(log_level=args.logLevel) # Setup MPL scenario # A Scenario is the fundamental building blocks of the VIE: Inputs, Signal Analysis, System Plant, and Output Sink vie = scenarios.MplScenario() # setup web interface vie.TrainingInterface = training.TrainingManagerWebsocket() vie.TrainingInterface.setup(port=uc.get_user_config_var('MobileApp.port', 9090)) vie.TrainingInterface.add_message_handler(vie.command_string) vie.setup() vie.TrainingData.motion_names = ('No Movement','Left','Right','Up','Down') custom_output = VieOutput() looper = FixedRateLoop(vie.Plant.dt) def loop_func(): # Run the actual model output = vie.update() # send gui updates msg = '<br>' + output['status'] # Classifier Status for src in vie.SignalSource: msg += '<br>MYO:' + src.get_status_msg() msg += '<br>' + time.strftime("%c") # Forward status message (voltage, temp, etc) to mobile app vie.TrainingInterface.send_message("sys_status", msg) # Send classifier output to mobile app (e.g. Elbow Flexion) vie.TrainingInterface.send_message("output_class", output['decision']) # Send motion training status to mobile app (e.g. No Movement [70] msg = '{} [{:.0f}]'.format(vie.training_motion, round(vie.TrainingData.get_totals(vie.training_id), -1)) vie.TrainingInterface.send_message("training_class", msg) custom_output.update(output['decision']) looper.loop(loop_func) vie.TrainingInterface.send_message("sys_status", 'CLOSED') vie.close()
def __init__(self, local_addr_str='//0.0.0.0:9029', remote_addr_str='//127.0.0.1:9027'): # Initialize superclass super(NfuSink, self).__init__() # mpl_status updated by heartbeat messages self.mpl_status = nfu.default_status_structure # battery samples will contain N most recent samples for averaging self.battery_samples = deque([], maxlen=15) self.reset_impedance = False # create a counter to delay how often CPU temperature is read and logged self.last_temperature = 0.0 self.last_temperature_counter = 0 self.stiffness_high = None self.stiffness_low = None self.joint_offset = None self.mpl_connection_check = None self.shutdown_voltage = None # RSA: moved this parameter out of the load function to not overwrite on reload from app # self.enable_impedance = None self.enable_impedance = get_user_config_var('MPL.enable_impedance', 0) self.impedance_level = 'high' # Options are low | high self.percepts = None # self.transport = open_nfu_comms.AsyncUdp(local_addr_str, remote_addr_str) # self.transport.name = 'AsyncOpenNfu' # self.transport.add_message_handler(self.parse_messages) # self.transport.connect() self.transport = udp_comms.Udp() self.transport.name = 'OpenNfu' self.transport.local_addr = get_address(local_addr_str) self.transport.remote_addr = get_address(remote_addr_str) self.transport.add_message_handler(self.parse_messages) self.load_config_parameters()
def load_config_parameters(self): # Load parameters from xml config file # initialize stiffness to global value (overwrite later if needed) s = get_user_config_var('GLOBAL_HAND_STIFFNESS_HIGH', 1.5) self.stiffness_high = [s] * MplId.NUM_JOINTS s = get_user_config_var('GLOBAL_HAND_STIFFNESS_LOW', 0.75) self.stiffness_low = [s] * MplId.NUM_JOINTS self.joint_offset = [0.0] * MplId.NUM_JOINTS # Upper Arm num_upper_arm_joints = 7 for i in range(num_upper_arm_joints): self.stiffness_high[i] = get_user_config_var( MplId(i).name + '_STIFFNESS_HIGH', 40.0) self.stiffness_low[i] = get_user_config_var( MplId(i).name + '_STIFFNESS_LOW', 20.0) for i in range(MplId.NUM_JOINTS): self.joint_offset[i] = np.deg2rad( get_user_config_var(MplId(i).name + '_OFFSET', 0.0)) # Hand if not get_user_config_var('GLOBAL_HAND_STIFFNESS_HIGH_ENABLE', 0): for i in range(num_upper_arm_joints, MplId.NUM_JOINTS): self.stiffness_high[i] = get_user_config_var( MplId(i).name + '_STIFFNESS_HIGH', 4.0) if not get_user_config_var('GLOBAL_HAND_STIFFNESS_LOW_ENABLE', 0): for i in range(num_upper_arm_joints, MplId.NUM_JOINTS): self.stiffness_low[i] = get_user_config_var( MplId(i).name + '_STIFFNESS_LOW', 4.0) self.shutdown_voltage = get_user_config_var('MPL.shutdown_voltage', 19.0) # self.enable_impedance = get_user_config_var('MPL.enable_impedance', 0) self.mpl_connection_check = get_user_config_var( 'MPL.connection_check', 1)
def command_string(self, value): """ This function accepts training commands Commands are strings with the following format: [CMD_TYPE]:[CMD_VALUE] [CMD_TYPE] options are: Cls - Indicates the cmd_value is the name of a motion class to be selected Cmd - Indicates the cmd_value is a command word. Options are: Shutdown - Send shutdown command to the OpenNFU Add - Begin adding data to the currently selected class Stop - Stop adding data to the currently selected class ClearClass - Clear the data for the currently selected class ClearAll - Clear all the labeled training data Train - Recompute the classifier based on the current data Save - Save all labeled training data to TRAINING_DATA.hdf5 file (note this also issues a backup) Backup - Copy the data in TRAINING_DATA.hdf5 to a timestamped backup file Pause - Temporarily Suspend Motion of the limb system SpeedUp - Increase speed of all arm joints SpeedDown - Decrease speed of all arm joints HandSpeedUp - Increase speed of hand motions HandSpeedDown - Decrease speed of hand motions PrecisionModeOff - Reset hand and arm speed to default values PrecisionModeOn - Set hand and arm speed to precision values AutoSaveOn - Automatically save training data when new data added AutoSaveOff - Turn off autosave feature TODO: add classifier options to train and switch between LDA, QDA, SVM, etc TODO: add reset to hand/arm speed """ # Commands should come in with colon operator # e.g. Cmd:Add or Cls:Elbow Flexion logging.info('Received new scenario command:' + value) parsed = value.split(':', 1) if not len(parsed) == 2: logging.warning('Invalid scenario command: ' + value) return else: cmd_type = parsed[0] cmd_data = parsed[1] if cmd_type == 'Cls': # Parse a Class Message # if cmd_data not in self.TrainingData.motion_names: # self.TrainingData.add_class(cmd_data) self.training_id = self.TrainingData.motion_names.index(cmd_data) self.training_motion = cmd_data self.add_data = False elif cmd_type == 'Log': # Parse a log message print("User inserted log message: " + cmd_data) logging.critical("User inserted log message: " + cmd_data) elif cmd_type == 'Time': # convert milliseconds string to decimal seconds browser_time = float(cmd_data) / 1000 utilities.sys_cmd.set_system_time(browser_time) elif cmd_type == 'Cmd': ################### # Training Options ################### if cmd_data == 'Add': self.add_data = True elif cmd_data == 'Stop': self.add_data = False self.SignalClassifier.fit() elif cmd_data == 'ClearClass': self.TrainingData.clear(self.training_id) self.SignalClassifier.fit() elif cmd_data == 'ClearAll': self.TrainingData.reset() self.SignalClassifier.fit() elif cmd_data == 'Train': self.SignalClassifier.fit() elif cmd_data == 'Save': self.TrainingData.copy() self.TrainingData.save() elif cmd_data == 'Backup': self.TrainingData.copy() elif cmd_data == 'AutoSaveOn': self.auto_save = True elif cmd_data == 'AutoSaveOff': self.auto_save = False ###################### # MPL Control Options ###################### elif cmd_data == 'ResetTorqueOn': self.DataSink.reset_impedance = True elif cmd_data == 'ResetTorqueOff': self.DataSink.reset_impedance = False elif cmd_data == 'ManualControlOn': self.manual_override = True elif cmd_data == 'ManualControlOff': self.manual_override = False elif cmd_data == 'JointPerceptsOn': self.enable_percept_stream = True elif cmd_data == 'JointPerceptsOff': self.enable_percept_stream = False elif cmd_data == 'AutoOpenOn': self.auto_open = True elif cmd_data == 'AutoOpenOff': self.auto_open = False elif cmd_data == 'ImpedanceOn': self.DataSink.enable_impedance = 1 elif cmd_data == 'ImpedanceOff': self.DataSink.enable_impedance = 0 elif cmd_data == 'ImpedanceLow': self.DataSink.impedance_level = 'low' elif cmd_data == 'ImpedanceHigh': self.DataSink.impedance_level = 'high' elif cmd_data == 'ReloadRoc': # Reload xml parameters and ROC Table # RSA: Update reload both ROC and xml config parameters read_user_config_file(reload=True) self.Plant.load_roc() self.Plant.load_config_parameters() self.DataSink.load_config_parameters() elif cmd_data == 'GotoHome': self.pause('All', True) angles = [0.0] * mpl.JointEnum.NUM_JOINTS self.DataSink.goto_smooth(angles) time.sleep(0.1) # synch percept position and plant position self.Plant.joint_position = self.DataSink.position['last_percept'][:] time.sleep(0.1) self.pause('All', False) elif cmd_data == 'GotoPark': self.pause('All', True) angles = self.DataSink.position['park'] self.DataSink.goto_smooth(angles) time.sleep(0.1) # synch percept position and plant position self.Plant.joint_position = self.DataSink.position['last_percept'][:] time.sleep(0.1) self.pause('All', False) ###################### # Myo Control Options ###################### elif cmd_data == 'RestartMyo1': utilities.sys_cmd.restart_myo() elif cmd_data == 'RestartMyo2': utilities.sys_cmd.restart_myo() elif cmd_data == 'ShutdownMyo1': logging.info('Received Myo 1 Shutdown Command') import socket sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) addr = utilities.get_address(get_user_config_var('MyoUdpClient.remote_address_1', '//127.0.0.1:16001')) sock.sendto(bytearray([1]), addr) sock.close() elif cmd_data == 'ShutdownMyo2': logging.info('Received Myo 2 Shutdown Command') import socket sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) addr = utilities.get_address(get_user_config_var('MyoUdpClient.remote_address_2', '//127.0.0.1:16001')) sock.sendto(bytearray([1]), addr) sock.close() elif cmd_data == 'ChangeMyoSet1': utilities.sys_cmd.change_myo(1) elif cmd_data == 'ChangeMyoSet2': utilities.sys_cmd.change_myo(2) elif cmd_data == 'NormUnity': self.Plant.ref_frame_upper = np.eye(4) self.Plant.ref_frame_lower = np.eye(4) ################# # System Options ################# elif cmd_data == 'Reboot': # utilities.reboot() # Try to set the limb state to soft reset try: self.DataSink.set_limb_soft_reset() except AttributeError: logging.warning('set_limb_soft_reset mode not defined') elif cmd_data == 'Shutdown': # utilities.shutdown() pass ################ # Speed Options ################ elif cmd_data == 'PrecisionModeOff': self.set_precision_mode(False) elif cmd_data == 'PrecisionModeOn': self.set_precision_mode(True) elif cmd_data == 'SpeedUp': self.gain(1.2) elif cmd_data == 'SpeedDown': self.gain(0.8) elif cmd_data == 'HandSpeedUp': self.hand_gain(1.2) elif cmd_data == 'HandSpeedDown': self.hand_gain(0.8) elif cmd_data == 'PauseHand': self.pause('Hand') elif cmd_data == 'PauseAllOn': self.pause('All', True) # Try to set the limb state to soft reset try: self.DataSink.set_limb_soft_reset() except AttributeError: logging.warning('set_limb_soft_reset mode not defined') elif cmd_data == 'PauseAllOff': # Synchronize current position # Wait for a new percept # Then set plant position to percept position # synchronize the data sink with the plant model if get_user_config_var('MPL.connection_check', 1): time.sleep(0.1) self.DataSink.wait_for_connection() # Synchronize joint positions if self.DataSink.position['last_percept'] is not None: for i in range(0, len(self.Plant.joint_position)): self.Plant.joint_position[i] = self.DataSink.position['last_percept'][i] time.sleep(0.1) self.pause('All', False) elif cmd_data == 'PauseHandOn': self.pause('Hand', True) elif cmd_data == 'PauseHandOff': self.pause('Hand', False) elif cmd_data == 'PauseWristFEOn': self.pause('WristFE', True) elif cmd_data == 'PauseWristFEOff': self.pause('WristFE', False) else: # It's ok to have commands that don't match here. another callback might use them # logging.info('Unknown scenario command: ' + cmd_data) pass elif cmd_type == 'Man': # Parse Manual Motion Command Message Type # Note the commands from the web app must match the Class Names # Don't accept manual commands until mode is enabled. Otherwise the command would be buffered # and the arm would start moving on command restore if not self.manual_override: return # Create a new timestep for the plant model (with all velocities zero) self.Plant.new_step() # If command is Stop, then just return (with zero velocity set) if cmd_data == 'Stop': return # parse manual command type as arm, grasp, etc class_info = controls.plant.class_map(cmd_data) if class_info['IsGrasp']: # the motion class is either a grasp type or hand open if class_info['GraspId'] is not None and self.Plant.grasp_position < 0.2: # change the grasp state if still early in the grasp motion self.Plant.grasp_id = class_info['GraspId'] self.Plant.set_grasp_velocity(class_info['Direction'] * self.hand_gain_value) else: # the motion class is an arm movement self.Plant.set_joint_velocity(class_info['JointId'], class_info['Direction'] * self.gain_value)
def run(self): """ Main function that involves setting up devices, looping at a fixed time interval, and performing cleanup """ import sys # setup main loop control print("") print("Running...") print("") sys.stdout.flush() # ########################## # Run the control loop # ########################## time_elapsed = 0.0 self.loop_time = time.strftime("%c") dt = self.Plant.dt print(dt) # synchronize the data sink with the plant model if get_user_config_var('MPL.connection_check', 1): logging.info('Checking for valid percepts...') wait_time_ms = 200 n = 0 while not self.DataSink.data_received(): time.sleep(wait_time_ms/1000) logging.info(f'Waiting {wait_time_ms} ms for valid percepts...') # Provide system status to app that we are alive but waiting for limb connection if self.TrainingInterface is not None: self.TrainingInterface.send_message("sys_status", 'Waiting for valid percepts' + '.'*n) n = (n+1) % 4 # After loop ends, synchronize joint positions for i in range(0, len(self.Plant.joint_position)): self.Plant.joint_position[i] = self.DataSink.position['last_percept'][i] while True: try: # Fixed rate loop. get start time, run model, get end time; delay for duration time_begin = time.perf_counter() # Run the actual model self.update() self.update_interface() time_end = time.perf_counter() time_elapsed = time_end - time_begin self.loop_dt_last = time_elapsed if dt > time_elapsed: time.sleep(dt - time_elapsed) # await asyncio.sleep(dt - time_elapsed) else: logging.warning("Timing Overload: {}".format(time_elapsed)) # await asyncio.sleep(0.001) pass # print('{0} dt={1:6.3f}'.format(output['decision'],time_elapsed)) except KeyboardInterrupt: break print("") print("Last time_elapsed was: ", time_elapsed) print("") print("Cleaning up...") print("") self.close()
def load_config_parameters(self): # Load parameters from xml config file for i in range(MplId.NUM_JOINTS): limit = user_config.get_user_config_var(MplId(i).name + '_LIMITS', (0.0, 30.0)) self.lower_limit[i] = np.deg2rad(limit[0]) self.upper_limit[i] = np.deg2rad(limit[1])
def create_instance_list(self, channels=8): sample_rate = get_user_config_var('FeatureExtract.sample_rate', 200) timestep = get_user_config_var('timestep', 0.02) steps_per_window = get_user_config_var('steps_per_window', 10) # feature extraction window slide & size in samples window_slide = floor(sample_rate * timestep) window_size = window_slide * steps_per_window if get_user_config_var("mav", True): mav = features.Mav(incremental=get_user_config_var( 'FeatureExtract.incremental_mav', False), window_size=window_size, window_slide=window_slide, channels=channels) self.vie.attach_feature(mav) if get_user_config_var("curve_len", True): curve_len = features.CurveLen(incremental=get_user_config_var( 'FeatureExtract.incremental_curve_len', False), window_size=window_size, window_slide=window_slide, channels=channels) self.vie.attach_feature(curve_len) if get_user_config_var("zc", True): zc = features.Zc(fs=sample_rate, zc_thresh=get_user_config_var( 'FeatureExtract.zc_threshold', 0.05), incremental=get_user_config_var( 'FeatureExtract.incremental_zc', False), window_size=window_size, window_slide=window_slide, channels=channels) self.vie.attach_feature(zc) if get_user_config_var("ssc", True): ssc = features.Ssc(fs=sample_rate, ssc_thresh=get_user_config_var( 'FeatureExtract.ssc_threshold', 0.05), incremental=get_user_config_var( 'FeatureExtract.incremental_ssc', False), window_size=window_size, window_slide=window_slide, channels=channels) self.vie.attach_feature(ssc) if get_user_config_var("wamp", False): wamp = features.Wamp(fs=sample_rate, wamp_thresh=get_user_config_var( 'FeatureExtract.wamp_threshold', 0.05)) self.vie.attach_feature(wamp) if get_user_config_var("var", False): var = features.Var() self.vie.attach_feature(var) if get_user_config_var("vorder", False): vorder = features.Vorder() self.vie.attach_feature(vorder) if get_user_config_var("logdetect", False): logdetect = features.LogDetect() self.vie.attach_feature(logdetect) if get_user_config_var("emghist", False): emghist = features.EmgHist() self.vie.attach_feature(emghist) if get_user_config_var("ar", False): ar = features.AR() self.vie.attach_feature(ar) if get_user_config_var("ceps", False): ceps = features.Ceps() self.vie.attach_feature(ceps)
def __init__(self): self.filename = 'TRAINING_DATA' self.file_ext = '.hdf5' # # Store class names # self.motion_names = 'No Movement' # TODO: Eliminate separate list for motion names # Names of potentially trained classes self.motion_names = ( 'No Movement', 'Shoulder Flexion', 'Shoulder Extension', 'Shoulder Adduction', 'Shoulder Abduction', 'Humeral Internal Rotation', 'Humeral External Rotation', 'Elbow Flexion', 'Elbow Extension', 'Wrist Rotate In', 'Wrist Rotate Out', 'Wrist Adduction', 'Wrist Abduction', 'Wrist Flex In', 'Wrist Extend Out', 'Hand Open', 'Spherical Grasp', 'Tip Grasp', 'Three Finger Pinch Grasp', 'Lateral Grasp', 'Cylindrical Grasp', 'Power Grasp', 'Point Grasp', 'Index', 'Middle', 'Ring', 'Little', 'Thumb', 'Ring-Middle', 'The Bird', 'Hang Loose', 'Thumbs Up', 'Peace', ) # Create lock to control write access to training data self.__lock = threading.Lock() self.num_channels = 0 # TODO: For now this was missing a split on comma. Future should get features based on what is enabled # self.features = get_user_config_var("features", "Mav,Curve_Len,Zc,Ssc").split() self.features = get_user_config_var("features", "Mav,Curve_Len,Zc,Ssc").split(',') self.data = [] # List of all feature extracted samples self.id = [] # List of class indices that each sample belongs to self.name = [] # Name of each class self.time_stamp = [] self.imu = [] self.num_samples = 0 self.reset()
def setup(self): """ Create the building blocks of the MiniVIE SignalSource - source of EMG data SignalClassifier - algorithm to classify emg into 'intent' Plant - Perform forward integration and apply joint limits DataSink - output destination of command signals (e.g. real or virtual arm) """ # from inputs import myo # from inputs import myo # Note that myo_async causes timing issue on DART from inputs.myo import myo_client from controls.plant import Plant from pattern_rec import features_selected ################################################ # Configure Inputs ################################################ input_device = get_user_config_var('input_device', 'myo') if input_device == 'myo': num_devices = get_user_config_var('MyoUdpClient.num_devices', 1) # Connect first armband local_1 = get_user_config_var('MyoUdpClient.local_address_1', '//0.0.0.0:15001') remote_1 = get_user_config_var('MyoUdpClient.remote_address_1', '//127.0.0.1:16001') self.attach_source(myo_client.MyoUdp(local_addr_str=local_1, remote_addr_str=remote_1)) if num_devices == 2: # Dual Armband Case local_2 = get_user_config_var('MyoUdpClient.local_address_2', '//0.0.0.0:15002') remote_2 = get_user_config_var('MyoUdpClient.local_address_2', '//127.0.0.1:16002') self.attach_source(myo_client.MyoUdp(local_addr_str=local_2, remote_addr_str=remote_2)) elif input_device == 'daq': from inputs import daqEMGDevice src = daqEMGDevice.DaqEMGDevice(get_user_config_var('DaqDevice.device_name_and_channels', 'Dev1/ai0:7')) self.attach_source(src) elif input_device == 'ctrl': from inputs import emg_device_client ws_address = get_user_config_var('EmgDevice.ws_address', 'ws://localhost:5678') buffer_len = get_user_config_var('EmgDevice.buffer_len', 200) src = emg_device_client.EmgSocket(source=ws_address, num_samples=buffer_len) self.SignalSource = [src] self.num_channels += src.num_channels self.futures = src.connect ################################################ # Configure Training Data Manager ################################################ self.TrainingData = pattern_rec.training_data.TrainingData() self.TrainingData.load() self.TrainingData.num_channels = self.num_channels ################################################ # Configure Pattern Recognition Classifier and Feature Extraction ################################################ self.FeatureExtract = pattern_rec.feature_extract.FeatureExtract() select_features = features_selected.FeaturesSelected(self.FeatureExtract) select_features.create_instance_list(self.num_channels) # Classifier parameters self.SignalClassifier = pattern_rec.classifier.Classifier(self.TrainingData) self.SignalClassifier.fit() ################################################ # Configure 'Plant' model to hold system state ################################################ # Plant maintains current limb state (positions) during velocity control filename = get_user_config_var('MPL.roc_table', '../../WrRocDefaults.xml') dt = get_user_config_var('timestep', 0.02) self.Plant = Plant(dt, filename) ################################################ # Configure Data Sink ################################################ # Sink is the output to outside world (in this case to VIE) # For MPL, this might be: real MPL/NFU, Virtual Arm, etc. data_sink = get_user_config_var('DataSink', 'Unity') if data_sink in ['Unity', 'UnityUdp', 'UnityAsyncio']: if data_sink == 'UnityAsyncio': from mpl.unity_asyncio import UnityUdp else: from mpl.unity import UnityUdp # The main output is to unity here, however output also supports additional 'ghost' limb control sink = UnityUdp(local_addr_str=get_user_config_var('UnityUdp.local_address', '//0.0.0.0:25001'), remote_addr_str=get_user_config_var('UnityUdp.remote_address', '//127.0.0.1:25000')) sink.connect() # send some default config parameters on setup for ghost arms (turn them off) enable = get_user_config_var('UnityUdp.ghost_default_enable', 0.0) color = get_user_config_var('UnityUdp.ghost_default_color', (0.3, 0.4, 0.5)) alpha = get_user_config_var('UnityUdp.ghost_default_alpha', 0.8) # TODO: this is a hard-coding to force the arms off on startup. not ideal... sink.config_port = 27000 sink.send_config_command(enable, color, alpha) sink.config_port = 27100 sink.send_config_command(enable, color, alpha) # Now read the user parameter for which arm the user wants to control sink.command_port = get_user_config_var('UnityUdp.ghost_command_port', 25010) sink.config_port = get_user_config_var('UnityUdp.ghost_config_port', 27000) elif data_sink == 'NfuUdp': from mpl.open_nfu.open_nfu_sink import NfuSink sink = NfuSink(local_addr_str=get_user_config_var('NfuUdp.local_address', '//0.0.0.0:9029'), remote_addr_str=get_user_config_var('NfuUdp.remote_address', '//127.0.0.1:9027')) sink.connect() else: import sys # unrecoverable logging.critical('Unmatched Data Sink from user_config: {}. Program Halted.'.format(data_sink)) self.close() sys.exit(1) self.DataSink = sink
def __init__(self): # import socket self.SignalSource = [] self.SignalClassifier = None self.FeatureExtract = None self.TrainingData = None self.TrainingInterface = None self.Plant = None self.DataSink = None # dict keeping track of the GUI connections # {websocket (websockethandler) : {connection id: (int), connection start time: (float)}} self.gui_connections = {} # Debug socket for streaming Features # self.DebugSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Loop control parameters self.loop_time = time.strftime("%c") # store the system time for timing status messages self.loop_dt_last = 0.0 # store the duration of the last execution loop for monitoring processor load self.loop_counter = 0 # count the number of loops to distribute messaging rate # Training parameters self.add_data = False # Control whether to add data samples on the current timestep self.add_data_last = False # Track whether data added on previous update, necessary to know when to autosave self.auto_save = True # Boolean, if true, will save out training data every time new data finished being added self.training_motion = 'No Movement' # Store the current motion name self.training_id = 0 # Store the current motion id self.num_channels = 0 self.auto_open = False # Automatically open hand if in rest state # Create a buffer for storing recent class decisions for majority voting self.decision_buffer = deque([], get_user_config_var('PatternRec.num_majority_votes', 25)) self.last_decision = None self.output = None # Will contain latest status message # User should access values through the is_paused method self.__pause = {'All': False, 'Arm': False, 'Hand': False, 'WristFE': False} # When set, this bypasses the classifier and add data methods to only send joint commands self.manual_override = False # Control whether percepts should be streamed via websocket to app (high bandwidth) self.enable_percept_stream = False # Control gains and speeds for precision control mode self.precision_mode = False self.gain_value = get_user_config_var('MPL.ArmSpeedDefault', 1.4) self.gain_value_last = self.gain_value self.gain_value_precision = get_user_config_var('MPL.ArmSpeedPrecision', 0.2) self.hand_gain_value = get_user_config_var('MPL.HandSpeedDefault', 1.2) self.hand_gain_value_last = self.hand_gain_value self.hand_gain_value_precision = get_user_config_var('MPL.HandSpeedPrecision', 0.15) # Motion Tracking parameters self.motion_track_enable = get_user_config_var('MotionTrack.enable', False) # Futures for event loop self.futures = None