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