diff --git a/pyrobolearn/tools/interfaces/sensors/myo/CHANGELOG.txt b/pyrobolearn/tools/interfaces/sensors/myo/CHANGELOG.txt new file mode 100644 index 0000000..0e7f5aa --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/myo/CHANGELOG.txt @@ -0,0 +1,7 @@ + +2014: Danny Zhu wrote the myo-raw package (https://github.com/dzhu/myo-raw). + +2015: Fernando Cosentino wrote the PyoConnect package (http://www.fernandocosentino.net/pyoconnect/) based on myo-raw. + +2019: Brian Delhaisse implemented the `onEMG` function, cleaned the ``MyoRaw`` and ``PyoConnectLib`` classes, added +a more complete documentation, and only print information if the ``verbose`` attribute is set. diff --git a/pyrobolearn/tools/interfaces/sensors/myo/LICENSE b/pyrobolearn/tools/interfaces/sensors/myo/LICENSE new file mode 100644 index 0000000..3478edc --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/myo/LICENSE @@ -0,0 +1,22 @@ +The MIT License (MIT) + +Copyright (c) 2014 Danny Zhu (for myo-raw) +Copyright (c) 2015 Fernando Cosentino (for PyoConnect) + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of +the Software, and to permit persons to whom the Software is furnished to do so, +subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS +FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER +IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + diff --git a/pyrobolearn/tools/interfaces/sensors/myo/PyoConnectLib.py b/pyrobolearn/tools/interfaces/sensors/myo/PyoConnectLib.py new file mode 100644 index 0000000..1f40037 --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/myo/PyoConnectLib.py @@ -0,0 +1,711 @@ +# uncompyle6 version 3.3.5 +# Python bytecode 2.7 (62211) +# Decompiled from: Python 2.7.12 (default, Nov 12 2018, 14:36:49) +# [GCC 5.4.0 20160609] +# Embedded file name: PyoConnectLib.py +# Compiled at: 2015-08-05 16:17:41 +""" + PyoConnect v0.1 + + Author: + Fernando Cosentino - fbcosentino@yahoo.com.br + + Official source: + http://www.fernandocosentino.net/pyoconnect + + Based on the work of dzhu: https://github.com/dzhu/myo-raw + + License: + Use at will, modify at will. Always keep my name in this file as original author. And that's it. + + Steps required (in a clean debian installation) to use this library: + // permission to ttyACM0 - must logout and login again on Linux + sudo usermod -a -G dialout $USER + + // dependencies + apt-get install python-pip + pip install pySerial --upgrade + pip install enum34 + pip install PyUserInput + apt-get install python-Xlib + + // now logout and login again + +Note that this file has been modified (mostly cleaned) with respect to the original by Brian Delhaisse. +""" +from __future__ import print_function +import sys +import time +from subprocess import Popen, PIPE +import re +import math + +try: + from pymouse import PyMouse + pmouse = PyMouse() +except: + print('PyMouse error: No mouse support') + pmouse = None +else: + try: + from pykeyboard import PyKeyboard + pkeyboard = PyKeyboard() + except: + print('PyKeyboard error: No keyboard support') + pkeyboard = None + +# from common import * +from myo_raw import MyoRaw, Pose, Arm, XDirection + + +class Myo(MyoRaw): + """Myo class""" + + def __init__(self, tty=None, verbose=False): + """ + Initialize the Myo instance. + + Args: + tty (str, None): tty (None, str): TTY (on Linux). You can check the list on a terminal by typing + `ls /dev/tty*`. By default, it will be '/dev/ttyACM0'. + verbose (bool): If True, it will print information about the state of the interface. This is let to the + programmer what he / she wishes to print. + """ + self.verbose = verbose + self.locked = True + self.use_lock = True + self.timed = True + self.lock_time = 1.0 + self.time_to_lock = self.lock_time + self.last_pose = -1 + self.last_tick = 0 + self.current_box = 0 + self.last_box = 0 + self.box_factor = 0.25 + self.current_arm = 0 + self.current_xdir = 0 + self.current_gyro = None + self.current_accel = None + self.current_roll = 0 + self.current_pitch = 0 + self.current_yaw = 0 + self.center_roll = 0 + self.center_pitch = 0 + self.center_yaw = 0 + self.first_rot = 0 + self.current_rot_roll = 0 + self.current_rot_pitch = 0 + self.current_rot_yaw = 0 + self.mov_history = '' + self.gest_history = '' + self.act_history = '' + if pmouse is not None: + self.x_dim, self.y_dim = pmouse.screen_size() + self.mx = self.x_dim / 2 + self.my = self.y_dim / 2 + self.centered = 0 + + self.current_emg_values = [] + self.bitmask_moving = 0 + + MyoRaw.__init__(self, tty=tty, verbose=verbose) + self.add_emg_handler(self.emg_handler) + self.add_arm_handler(self.arm_handler) + self.add_imu_handler(self.imu_handler) + self.add_pose_handler(self.pose_handler) + self.onEMG = None + self.onPoseEdge = None + self.onPoseEdgeList = [] + self.onLock = None + self.onLockList = [] + self.onUnlock = None + self.onUnlockList = [] + self.onPeriodic = None + self.onPeriodicList = [] + self.onWear = None + self.onWearList = [] + self.onUnwear = None + self.onUnwearList = [] + self.onBoxChange = None + self.onBoxChangeList = [] + return + + def check_myo_around(self): + self.bt.end_scan() + self.bt.disconnect(0) + self.bt.disconnect(1) + self.bt.disconnect(2) + self.bt.discover() + p = self.bt.recv_packet(1) + try: + pl = p.payload + except: + pl = '' + + if pl.endswith('\x06BH\x12J\x7f,HG\xb9\xde\x04\xa9\x01\x00\x06\xd5'): + self.bt.end_scan() + return True + else: + return False + + def tick(self): + now = time.time() + if now - self.last_tick >= 0.01: + if self.onPeriodic is not None: + self.onPeriodic() + for h in self.onPeriodicList: + h() + + if self.use_lock and self.locked == False and self.timed: + if self.time_to_lock <= 0: + if self.verbose: + print('Locked') + self.locked = True + self.vibrate(1) + self.time_to_lock = self.lock_time + if self.onLock is not None: + self.onLock() + for h in self.onLockList: + h() + + else: + self.time_to_lock -= 0.01 + self.last_tick = now + return + + def clear_handle_lists(self): + self.onPoseEdgeList = [] + self.onLockList = [] + self.onUnlockList = [] + self.onPeriodicList = [] + self.onWearList = [] + self.onUnwearList = [] + self.onBoxChangeList = [] + self.emg_handlers = [] + + def Add_onPoseEdge(self, h): + self.onPoseEdgeList.append(h) + + def Add_onLock(self, h): + self.onLockList.append(h) + + def Add_onUnlock(self, h): + self.onUnlockList.append(h) + + def Add_onPeriodic(self, h): + self.onPeriodicList.append(h) + + def Add_onWear(self, h): + self.onWearList.append(h) + + def Add_onUnwear(self, h): + self.onUnwearList.append(h) + + def Add_onBoxChange(self, h): + self.onBoxChangeList.append(h) + + def emg_handler(self, emg, moving): + """EMG handler.""" + # if self.onEMG is not None: + # self.onEMG(emg, moving) + self.current_emg_values = emg + self.bitmask_moving = moving + return + + def arm_handler(self, arm, xdir): + """Arm handler.""" + if arm == Arm(0): + self.current_arm = 'unknown' + elif arm == Arm(1): + self.current_arm = 'right' + elif arm == Arm(2): + self.current_arm = 'left' + if xdir == XDirection(0): + self.current_xdir = 'unknown' + elif xdir == XDirection(1): + self.current_xdir = 'towardWrist' + elif xdir == XDirection(2): + self.current_xdir = 'towardElbow' + if Arm(arm) == 0: + if self.onUnwear is not None: + self.onUnwear() + for h in self.onUnwearList: + h() + + elif self.onWear is not None: + self.onWear(self.current_arm, self.current_xdir) + else: + for h in self.onWearList: + h(self.current_arm, self.current_xdir) + + return + + def imu_handler(self, quat, acc, gyro): + """IMU handler""" + q0, q1, q2, q3 = quat + q0 = q0 / 16384.0 + q1 = q1 / 16384.0 + q2 = q2 / 16384.0 + q3 = q3 / 16384.0 + self.current_roll = math.atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)) + self.current_pitch = -math.asin(max(-1.0, min(1.0, 2.0 * (q0 * q2 - q3 * q1)))) + self.current_yaw = -math.atan2(2.0 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2 * q2 + q3 * q3)) + self.current_rot_roll = self.angle_dif(self.current_roll, self.center_roll) + self.current_rot_yaw = self.angle_dif(self.current_yaw, self.center_yaw) + self.current_rot_pitch = self.angle_dif(self.current_pitch, self.center_pitch) + g0, g1, g2 = gyro + g0 = g0 / 16.0 + g1 = g1 / 16.0 + g2 = g2 / 16.0 + self.current_gyro = (g0, g1, g2) + ac0, ac1, ac2 = acc + ac0 = ac0 / 2048.0 + ac1 = ac1 / 2048.0 + ac2 = ac2 / 2048.0 + self.current_accel = (ac0, ac1, ac2) + if self.first_rot == 0: + self.rotSetCenter() + self.first_rot = 1 + self.current_box = self.getBox() + if self.current_box != self.last_box: + self.mov_history = str(self.mov_history[-100:]) + str(self.current_box) + self.act_history = str(self.act_history[-100:]) + str(self.current_box) + if self.onBoxChange is not None: + self.onBoxChange(self.last_box, 'off') + self.onBoxChange(self.current_box, 'on') + for h in self.onBoxChangeList: + h(self.last_box, 'off') + h(self.current_box, 'on') + + self.last_box = self.current_box + return + + def pose_handler(self, p): + """Pose handler.""" + if p == Pose(0): + pn = 0 + elif p == Pose(1): + pn = 1 + elif p == Pose(2): + pn = 2 + elif p == Pose(3): + pn = 3 + elif p == Pose(4): + pn = 4 + elif p == Pose(5): + pn = 5 + else: + pn = 6 + if pn != self.last_pose: + self.gest_history = str(self.gest_history[-100:]) + str(self.PoseToChar(pn)) + self.act_history = str(self.act_history[-100:]) + str(self.PoseToChar(pn)) + if self.locked == False: + self.time_to_lock = self.lock_time + if self.last_pose > -1: + if self.onPoseEdge is not None: + self.onPoseEdge(self.PoseToStr(self.last_pose), 'off') + for h in self.onPoseEdgeList: + h(self.PoseToStr(pn), 'off') + + if self.onPoseEdge is not None: + self.onPoseEdge(self.PoseToStr(pn), 'on') + for h in self.onPoseEdgeList: + h(self.PoseToStr(pn), 'on') + + self.last_pose = pn + if pn == 5 and self.locked and self.use_lock: + self.locked = False + self.vibrate(1) + if self.verbose: + print('unlock') + if self.onUnlock is not None: + self.onUnlock() + for h in self.onUnlockList: + h() + + return + + @property + def arm(self): + """Get the arm.""" + return self.current_arm + + @property + def x_direction(self): + return self.current_xdir + + @property + def gyro(self): + return self.current_gyro + + @property + def accel(self): + return self.current_accel + + @property + def time_milliseconds(self): + return round(time.time() * 1000) + + @property + def roll(self): + return self.current_roll + + @property + def pitch(self): + return self.current_pitch + + @property + def yaw(self): + return self.current_yaw + + def setLockingPolicy(self, policy): + if policy == 'none': + self.use_lock = False + elif policy == 'standard': + self.use_lock = True + + def lock(self): + self.locked = True + self.vibrate(1) + if self.onLock is not None: + self.onLock() + for h in self.onLockList: + h() + + return + + def unlock(self, unlock_type): + if unlock_type == 'timed': + self.vibrate(1) + self.locked = False + self.timed = True + if unlock_type == 'hold': + self.vibrate(1) + self.locked = False + self.timed = False + + def isUnlocked(self): + if self.locked: + return False + else: + return True + + def notifyUserAction(self): + self.vibrate(1) + + def keyboard(self, kkey, kedge, kmod): + if pkeyboard is not None: + tkey = kkey + if tkey == 'left_arrow': + tkey = pkeyboard.left_key + if tkey == 'right_arrow': + tkey = pkeyboard.right_key + if tkey == 'up_arrow': + tkey = pkeyboard.up_key + if tkey == 'down_arrow': + tkey = pkeyboard.down_key + if tkey == 'space': + pass + if tkey == 'return': + tkey = pkeyboard.return_key + if tkey == 'escape': + tkey = pkeyboard.escape_key + if kmod == 'left_shift': + pkeyboard.press_key(pkeyboard.shift_l_key) + if kmod == 'right_shift': + pkeyboard.press_key(pkeyboard.shift_r_key) + if kmod == 'left_control': + pkeyboard.press_key(pkeyboard.control_l_key) + if kmod == 'right_control': + pkeyboard.press_key(pkeyboard.control_r_key) + if kmod == 'left_alt': + pkeyboard.press_key(pkeyboard.alt_l_key) + if kmod == 'right_alt': + pkeyboard.press_key(pkeyboard.alt_r_key) + if kmod == 'left_win': + pkeyboard.press_key(pkeyboard.super_l_key) + if kmod == 'right_win': + pkeyboard.press_key(pkeyboard.super_r_key) + if kedge == 'down': + pkeyboard.press_key(tkey) + elif kedge == 'up': + pkeyboard.release_key(tkey) + elif kedge == 'press': + pkeyboard.tap_key(tkey) + if kmod == 'left_shift': + pkeyboard.release_key(pkeyboard.shift_l_key) + if kmod == 'right_shift': + pkeyboard.release_key(pkeyboard.shift_r_key) + if kmod == 'left_control': + pkeyboard.release_key(pkeyboard.control_l_key) + if kmod == 'right_control': + pkeyboard.release_key(pkeyboard.control_r_key) + if kmod == 'left_alt': + pkeyboard.release_key(pkeyboard.alt_l_key) + if kmod == 'right_alt': + pkeyboard.release_key(pkeyboard.alt_r_key) + if kmod == 'left_win': + pkeyboard.release_key(pkeyboard.super_l_key) + if kmod == 'right_win': + pkeyboard.release_key(pkeyboard.super_r_key) + return + + def centerMousePosition(self): + if pmouse is not None: + x_dim, y_dim = pmouse.screen_size() + pmouse.move(x_dim / 2, y_dim / 2) + return + + def mouse(self, button, edge, mod): + if pmouse is not None: + mpos = pmouse.position() + if button == 'left': + mbut = 1 + elif button == 'right': + mbut = 2 + elif button == 'center': + mbut = 3 + else: + mbut = 1 + if edge == 'down': + pmouse.press(mpos[0], mpos[1], mbut) + elif edge == 'up': + pmouse.release(mpos[0], mpos[1], mbut) + elif edge == 'click': + pmouse.click(mpos[0], mpos[1], mbut) + return + + @property + def pose(self): + return self.PoseToStr(self.last_pose) + + def getPoseSide(self): + if self.last_pose == 2 and self.current_arm == 'right' or self.last_pose == 3 and self.current_arm == 'left': + return 'waveLeft' + if self.last_pose == 3 and self.current_arm == 'right' or self.last_pose == 2 and self.current_arm == 'left': + return 'waveRight' + return self.PoseToStr(self.last_pose) + + def isLocked(self): + return self.locked + + def mouseMove(self, x, y): + if pmouse is not None: + pmouse.move(x, y) + return + + def title_contains(self, text): + window_str = self.get_active_window_title() + if window_str.find(text) > -1: + return True + else: + return False + + def class_contains(self, text): + window_str = self.get_active_window_class() + if window_str.find(text) > -1: + return True + else: + return False + + def rotSetCenter(self): + self.center_roll = self.current_roll + self.center_pitch = self.current_pitch + self.center_yaw = self.current_yaw + + def rotRoll(self): + return self.current_rot_roll + + def rotPitch(self): + return self.current_rot_pitch + + def rotYaw(self): + return self.angle_dif(self.current_yaw, self.center_yaw) + + def getBox(self): + if self.current_rot_pitch > self.box_factor: + if self.current_rot_yaw > self.box_factor: + return 2 + else: + if self.current_rot_yaw < -self.box_factor: + return 8 + return 1 + + elif self.current_rot_pitch < -self.box_factor: + if self.current_rot_yaw > self.box_factor: + return 4 + else: + if self.current_rot_yaw < -self.box_factor: + return 6 + return 5 + + elif self.current_rot_yaw > self.box_factor: + return 3 + else: + if self.current_rot_yaw < -self.box_factor: + return 7 + return 0 + + def getHBox(self): + if self.current_rot_yaw > self.box_factor: + return 1 + else: + if self.current_rot_yaw < -self.box_factor: + return -1 + return 0 + + def getVBox(self): + if self.current_rot_pitch > self.box_factor: + return 1 + else: + if self.current_rot_pitch < -self.box_factor: + return -1 + return 0 + + def clearHistory(self): + self.mov_history = '' + self.gest_history = '' + self.act_history = '' + + def getLastMovements(self, num): + if num >= 0: + return self.mov_history[-num:] + else: + return self.mov_history + + def getLastGestures(self, num): + if num >= 0: + return self.gest_history[-num:] + else: + return self.gest_history + + def getLastActions(self, num): + if num >= 0: + return self.act_history[-num:] + else: + return self.act_history + + def PoseToStr(self, posenum): + """Return a string describing the given pose id. + + Check: https://support.getmyo.com/hc/article_attachments/115012009363/myo-for-education-lesson-5.pdf + """ + if posenum == 0: + return 'rest' + else: + if posenum == 1: + return 'fist' + if posenum == 2: + return 'waveIn' + if posenum == 3: + return 'waveOut' + if posenum == 4: + return 'fingersSpread' + if posenum == 5: + return 'doubleTap' + return 'unknown' + + def PoseToChar(self, posenum): + """Return a char describing the given pose id. + + Here is what each letter stands for: + 'R' --> rest + 'F' --> fist + 'I' --> wave in + 'O' --> wave out + 'S' --> fingers spread + 'D' --> double tap + 'U' --> unknown + + Check: https://support.getmyo.com/hc/article_attachments/115012009363/myo-for-education-lesson-5.pdf + """ + if posenum == 0: + return 'R' + else: + if posenum == 1: + return 'F' + if posenum == 2: + return 'I' + if posenum == 3: + return 'O' + if posenum == 4: + return 'S' + if posenum == 5: + return 'D' + return 'U' + + def limit_angle(self, angle): + if angle > math.pi: + return angle - 2.0 * math.pi + if angle < -2.0 * math.pi: + return angle + 2.0 * math.pi + return angle + + def angle_dif(self, angle, ref): + if ref >= 0: + if angle >= 0: + return self.limit_angle(angle - ref) + else: + if angle >= ref - math.pi: + return self.limit_angle(angle - ref) + return self.limit_angle(angle + 2.0 * math.pi - ref) + + elif angle <= 0: + return self.limit_angle(angle - ref) + else: + if angle <= ref + math.pi: + return self.limit_angle(angle - ref) + return self.limit_angle(angle - 2.0 * math.pi - ref) + + def get_active_window_title(self): + try: + root = Popen(['xprop', '-root', '_NET_ACTIVE_WINDOW'], stdout=PIPE) + for line in root.stdout: + mw = re.search('^_NET_ACTIVE_WINDOW.* ([\\w]+)$', line) + if mw is not None: + id_ = mw.group(1) + id_w = Popen(['xprop', '-id', id_, 'WM_NAME'], stdout=PIPE) + break + + if id_w is not None: + for line in id_w.stdout: + match = re.match('WM_NAME\\(\\w+\\) = (?P.+)$', line) + if match is not None: + return match.group('name') + + return '' + except: + return '' + + return + + def get_active_window_class(self): + try: + root = Popen(['xprop', '-root', '_NET_ACTIVE_WINDOW'], stdout=PIPE) + for line in root.stdout: + mw = re.search('^_NET_ACTIVE_WINDOW.* ([\\w]+)$', line) + if mw is not None: + id_ = mw.group(1) + id_w = Popen(['xprop', '-id', id_, 'WM_CLASS'], stdout=PIPE) + break + + if id_w is not None: + for line in id_w.stdout: + match = re.match('WM_CLASS\\(\\w+\\) = (?P.+)$', line) + if match is not None: + return match.group('name') + + return '' + except: + return '' + + return + + +if __name__ == '__main__': + tty = sys.argv[1] if len(sys.argv) >= 2 else None + m = Myo(tty=tty, verbose=True) + m.connect() + + while True: + m.run() diff --git a/pyrobolearn/tools/interfaces/sensors/myo/__init__.py b/pyrobolearn/tools/interfaces/sensors/myo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/interfaces/sensors/myo/common.py b/pyrobolearn/tools/interfaces/sensors/myo/common.py new file mode 100644 index 0000000..39d03a6 --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/myo/common.py @@ -0,0 +1,10 @@ +import struct + +def pack(fmt, *args): + return struct.pack('<' + fmt, *args) + +def unpack(fmt, *args): + return struct.unpack('<' + fmt, *args) + +def text(scr, font, txt, pos, clr=(255,255,255)): + scr.blit(font.render(txt, True, clr), pos) diff --git a/pyrobolearn/tools/interfaces/sensors/myo/install_myo_ubuntu.txt b/pyrobolearn/tools/interfaces/sensors/myo/install_myo_ubuntu.txt new file mode 100644 index 0000000..03e70e3 --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/myo/install_myo_ubuntu.txt @@ -0,0 +1,26 @@ +Installation of Myo Armband on Ubuntu 16.04/18.04 +================================================= + +The following instructions are based on the instructions given in: http://www.fernandocosentino.net/pyoconnect/ + +In a terminal: + +# plug bluetooth adapter +# permission to ttyACM0 - must logout from your session then login again, or reboot your system. +sudo usermod -a -G dialout $USER + +# dependencies +sudo apt-get install python-pip +sudo pip install pySerial --upgrade +sudo pip install enum34 +sudo pip install PyUserInput +sudo apt-get install python-xlib +sudo apt-get install python-tk + +# now logout from your session then login again, or reboot your system + +Check that the dialout group has been added: +$ groups + +Check that you can see ttyACM0: +$ ls /dev/ttyACM* diff --git a/pyrobolearn/tools/interfaces/sensors/myo/myo_raw.py b/pyrobolearn/tools/interfaces/sensors/myo/myo_raw.py new file mode 100644 index 0000000..658a301 --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/myo/myo_raw.py @@ -0,0 +1,582 @@ +#!/usr/bin/env python +""" +Original by dzhu: https://github.com/dzhu/myo-raw + +Edited by Fernando Cosentino: http://www.fernandocosentino.net/pyoconnect + +Further cleaned by Brian Delhaisse. +""" + + +from __future__ import print_function + +import enum +import re +import struct +import sys +import threading +import time + +import serial +from serial.tools.list_ports import comports + +from common import * + + +def multichr(ords): + if sys.version_info[0] >= 3: + return bytes(ords) + else: + return ''.join(map(chr, ords)) + + +def multiord(b): + if sys.version_info[0] >= 3: + return list(b) + else: + return map(ord, b) + + +class Arm(enum.Enum): + UNKNOWN = 0 + RIGHT = 1 + LEFT = 2 + + +class XDirection(enum.Enum): + UNKNOWN = 0 + X_TOWARD_WRIST = 1 + X_TOWARD_ELBOW = 2 + + +class Pose(enum.Enum): + REST = 0 + FIST = 1 + WAVE_IN = 2 + WAVE_OUT = 3 + FINGERS_SPREAD = 4 + THUMB_TO_PINKY = 5 + UNKNOWN = 255 + + +class Packet(object): + def __init__(self, ords): + self.typ = ords[0] + self.cls = ords[2] + self.cmd = ords[3] + self.payload = multichr(ords[4:]) + + def __repr__(self): + return 'Packet(%02X, %02X, %02X, [%s])' % \ + (self.typ, self.cls, self.cmd, + ' '.join('%02X' % b for b in multiord(self.payload))) + + +class BT(object): + """Implements the non-Myo-specific details of the Bluetooth protocol.""" + + def __init__(self, tty): + self.ser = serial.Serial(port=tty, baudrate=9600, dsrdtr=1) + self.buf = [] + self.lock = threading.Lock() + self.handlers = [] + + # internal data-handling methods + def recv_packet(self, timeout=None): + """Receive a packet and call the event handlers.""" + t0 = time.time() + self.ser.timeout = None + while timeout is None or time.time() < t0 + timeout: + if timeout is not None: + self.ser.timeout = t0 + timeout - time.time() + c = self.ser.read() + if not c: + return None + + ret = self.proc_byte(ord(c)) + if ret: + if ret.typ == 0x80: + self.handle_event(ret) + return ret + + def recv_packets(self, timeout=.5): + """Receive multiple packets until the specified timeout.""" + res = [] + t0 = time.time() + while time.time() < t0 + timeout: + p = self.recv_packet(t0 + timeout - time.time()) + if not p: + return res + res.append(p) + return res + + def proc_byte(self, c): + """Process the received bytes.""" + if not self.buf: + if c in [0x00, 0x80, 0x08, 0x88]: + self.buf.append(c) + return None + elif len(self.buf) == 1: + self.buf.append(c) + self.packet_len = 4 + (self.buf[0] & 0x07) + self.buf[1] + return None + else: + self.buf.append(c) + + if self.packet_len and len(self.buf) == self.packet_len: + p = Packet(self.buf) + self.buf = [] + return p + return None + + def handle_event(self, p): + """Send the processed received packet to the event/data handlers.""" + for h in self.handlers: + h(p) + + def add_handler(self, h): + """Add an event handler to the list of event/data handlers""" + self.handlers.append(h) + + def remove_handler(self, h): + """Try to remove the first instance of the specified event/data handler""" + try: + self.handlers.remove(h) + except ValueError: + pass + + def wait_event(self, cls, cmd): + """Wait for an event""" + res = [None] + + def h(p): + if p.cls == cls and p.cmd == cmd: + res[0] = p + + self.add_handler(h) + while res[0] is None: + self.recv_packet() + self.remove_handler(h) + return res[0] + + # specific BLE commands + def connect(self, addr): + return self.send_command(6, 3, pack('6sBHHHH', multichr(addr), 0, 6, 6, 64, 0)) + + def get_connections(self): + return self.send_command(0, 6) + + def discover(self): + return self.send_command(6, 2, b'\x01') + + def end_scan(self): + return self.send_command(6, 4) + + def disconnect(self, h): + return self.send_command(3, 0, pack('B', h)) + + def read_attr(self, con, attr): + self.send_command(4, 4, pack('BH', con, attr)) + return self.wait_event(4, 5) + + def write_attr(self, con, attr, val): + self.send_command(4, 5, pack('BHB', con, attr, len(val)) + val) + return self.wait_event(4, 1) + + def send_command(self, cls, cmd, payload=b'', wait_resp=True): + s = pack('4B', 0, len(payload), cls, cmd) + payload + self.ser.write(s) + + while True: + p = self.recv_packet() + + # no timeout, so p won't be None + if p.typ == 0: + return p + + # not a response: must be an event + self.handle_event(p) + + +class MyoRaw(object): + """Implements the Myo-specific communication protocol.""" + + def __init__(self, tty=None, verbose=False): + """ + Initialize the MyoRaw instance. + + Args: + tty (str, None): tty (None, str): TTY (on Linux). You can check the list on a terminal by typing + `ls /dev/tty*`. By default, it will be '/dev/ttyACM0'. + verbose (bool): If True, it will print information about the state of the interface. This is let to the + programmer what he / she wishes to print. + """ + if tty is None: + tty = self.detect_tty() + if tty is None: + raise ValueError('Myo dongle not found!') + + self.verbose = verbose + + self.bt = BT(tty) + self.conn = None + self.emg_handlers = [] + self.imu_handlers = [] + self.arm_handlers = [] + self.pose_handlers = [] + + def detect_tty(self): + """Detect automatically the tty to connect to the Myo armband.""" + for p in comports(): + if re.search(r'PID=2458:0*1', p[2]): + if self.verbose: + print('using device: {}'.format(p[0])) + return p[0] + + return None + + def run(self, timeout=None): + """Run the MyoRaw. This has to be called at each time step.""" + self.bt.recv_packet(timeout) + + def connect(self): + """Connect to the Myo armband.""" + # stop everything from before + self.bt.end_scan() + self.bt.disconnect(0) + self.bt.disconnect(1) + self.bt.disconnect(2) + + # start scanning + if self.verbose: + print('scanning...') + self.bt.discover() + while True: + p = self.bt.recv_packet() + if self.verbose: + print('scan response: {}'.format(p)) + + if p.payload.endswith(b'\x06\x42\x48\x12\x4A\x7F\x2C\x48\x47\xB9\xDE\x04\xA9\x01\x00\x06\xD5'): + addr = list(multiord(p.payload[2:8])) + break + self.bt.end_scan() + + # connect and wait for status event + conn_pkt = self.bt.connect(addr) + self.conn = multiord(conn_pkt.payload)[-1] + self.bt.wait_event(3, 0) + + # get firmware version + fw = self.read_attr(0x17) + _, _, _, _, v0, v1, v2, v3 = unpack('BHBBHHHH', fw.payload) + if self.verbose: + print('firmware version: %d.%d.%d.%d' % (v0, v1, v2, v3)) + + self.old = (v0 == 0) + + if self.old: + # don't know what these do; Myo Connect sends them, though we get data fine without them + self.write_attr(0x19, b'\x01\x02\x00\x00') + self.write_attr(0x2f, b'\x01\x00') + self.write_attr(0x2c, b'\x01\x00') + self.write_attr(0x32, b'\x01\x00') + self.write_attr(0x35, b'\x01\x00') + + # enable EMG data + self.write_attr(0x28, b'\x01\x00') + # enable IMU data + self.write_attr(0x1d, b'\x01\x00') + + # Sampling rate of the underlying EMG sensor, capped to 1000. If it's + # less than 1000, emg_hz is correct. If it is greater, the actual + # framerate starts dropping inversely. Also, if this is much less than + # 1000, EMG data becomes slower to respond to changes. In conclusion, + # 1000 is probably a good value. + C = 1000 + emg_hz = 50 + # strength of low-pass filtering of EMG data + emg_smooth = 100 + + imu_hz = 50 + + # send sensor parameters, or we don't get any data + self.write_attr(0x19, pack('BBBBHBBBBB', 2, 9, 2, 1, C, emg_smooth, C // emg_hz, imu_hz, 0, 0)) + + else: + name = self.read_attr(0x03) + if self.verbose: + print('device name: %s' % name.payload) + + # enable IMU data + self.write_attr(0x1d, b'\x01\x00') + # enable on/off arm notifications + self.write_attr(0x24, b'\x02\x00') + + # self.write_attr(0x19, b'\x01\x03\x00\x01\x01') + self.start_raw() + + # add data handlers (which are called each time we receive a packet) + def handle_data(p): + if (p.cls, p.cmd) != (4, 5): + return + + c, attr, typ = unpack('BHB', p.payload[:4]) + pay = p.payload[5:] + + if attr == 0x27: # EMG + vals = unpack('8HB', pay) + # not entirely sure what the last byte is, but it's a bitmask that + # seems to indicate which sensors think they're being moved around or + # something + emg = vals[:8] + moving = vals[8] + print("EEEEMMMMMMGGGGGG: ", emg) + self.on_emg(emg, moving) + + elif attr == 0x1c: # IMU + vals = unpack('10h', pay) + quat = vals[:4] + acc = vals[4:7] + gyro = vals[7:10] + self.on_imu(quat, acc, gyro) + + elif attr == 0x23: # Arm + Pose + typ, val, xdir, _, _, _ = unpack('6B', pay) + + if typ == 1: # on arm + self.on_arm(Arm(val), XDirection(xdir)) + elif typ == 2: # removed from arm + self.on_arm(Arm.UNKNOWN, XDirection.UNKNOWN) + elif typ == 3: # pose + self.on_pose(Pose(val)) + else: + if self.verbose: + print('data with unknown attr: %02X %s' % (attr, p)) + + # add the data handler to the list of handlers processed by the Bluetooth + self.bt.add_handler(handle_data) + + def write_attr(self, attr, val): + """Write the given value to the specified attribute.""" + if self.conn is not None: + self.bt.write_attr(self.conn, attr, val) + + def read_attr(self, attr): + """Read the specified attribute.""" + if self.conn is not None: + return self.bt.read_attr(self.conn, attr) + return None + + def disconnect(self): + """Disconnect from the myo armband.""" + if self.conn is not None: + self.bt.disconnect(self.conn) + + def start_raw(self): + """Sending this sequence for v1.0 firmware seems to enable both raw data and + pose notifications. + """ + + self.write_attr(0x28, b'\x01\x00') # EMG? + # self.write_attr(0x19, b'\x01\x03\x01\x01\x00') + self.write_attr(0x19, b'\x01\x03\x01\x01\x01') + + def mc_start_collection(self): + """Myo Connect sends this sequence (or a reordering) when starting data + collection for v1.0 firmware; this enables raw data but disables arm and + pose notifications. + """ + + self.write_attr(0x28, b'\x01\x00') + self.write_attr(0x1d, b'\x01\x00') + self.write_attr(0x24, b'\x02\x00') + self.write_attr(0x19, b'\x01\x03\x01\x01\x01') + self.write_attr(0x28, b'\x01\x00') + self.write_attr(0x1d, b'\x01\x00') + self.write_attr(0x19, b'\x09\x01\x01\x00\x00') + self.write_attr(0x1d, b'\x01\x00') + self.write_attr(0x19, b'\x01\x03\x00\x01\x00') + self.write_attr(0x28, b'\x01\x00') + self.write_attr(0x1d, b'\x01\x00') + self.write_attr(0x19, b'\x01\x03\x01\x01\x00') + + def mc_end_collection(self): + """Myo Connect sends this sequence (or a reordering) when ending data collection + for v1.0 firmware; this reenables arm and pose notifications, but + doesn't disable raw data. + """ + + self.write_attr(0x28, b'\x01\x00') + self.write_attr(0x1d, b'\x01\x00') + self.write_attr(0x24, b'\x02\x00') + self.write_attr(0x19, b'\x01\x03\x01\x01\x01') + self.write_attr(0x19, b'\x09\x01\x00\x00\x00') + self.write_attr(0x1d, b'\x01\x00') + self.write_attr(0x24, b'\x02\x00') + self.write_attr(0x19, b'\x01\x03\x00\x01\x01') + self.write_attr(0x28, b'\x01\x00') + self.write_attr(0x1d, b'\x01\x00') + self.write_attr(0x24, b'\x02\x00') + self.write_attr(0x19, b'\x01\x03\x01\x01\x01') + + def vibrate(self, length): + """ + Vibrate the myo armband for the specified length. + + Args: + length (int): integer between 1 and 4. + """ + if length in xrange(1, 4): + # first byte tells it to vibrate; purpose of second byte is unknown + self.write_attr(0x19, pack('3B', 3, 1, length)) + + def add_emg_handler(self, h): + """Add an EMG handler function/method which will be called each time we receive a packet. The handler function + has to accept as inputs two parameters where the first one will be a list of 8 EMG sensor values, and the + other will be an int to specify if it is moving or not. + + Args: + h (callable): EMG handler function. + """ + self.emg_handlers.append(h) + + def add_imu_handler(self, h): + """Add an IMU handler function/method which will be called each time we receive a packet. The handler function + has to accept as inputs 3 parameters where the first one will be the orientation expressed as a quaternion, + the 3 acceleration values returned by the accelerometer, and the 3 angular velocity values returned by the + gyro. + + Args: + h (callable): IMU handler function. + """ + self.imu_handlers.append(h) + + def add_pose_handler(self, h): + """Add a Pose handler function/method which will be called each time we receive a packet. The handler function + has to accept as input one parameter which is the pose (an instance of ``Pose``). + + Args: + h (callable): pose handler function. + """ + self.pose_handlers.append(h) + + def add_arm_handler(self, h): + """ + Add an Arm handler function/method which will be called each time we receive a packet. The handler function + has to accept as inputs two parameters where the first one will be the arm (an instance of ``Arm``), and + the second will be the x direction (an instance of ``XDirection``). + + Args: + h (callable): arm handler function. + """ + self.arm_handlers.append(h) + + def on_emg(self, emg, moving): + """ + Call each EMG handler function, and pass them the EMG values and if moving or not. + """ + for h in self.emg_handlers: + h(emg, moving) + + def on_imu(self, quat, acc, gyro): + """ + Call each IMU handler function, and pass them the IMU values (quaternion, acceleration, gyro). + """ + for h in self.imu_handlers: + h(quat, acc, gyro) + + def on_pose(self, p): + """ + Call each Pose handler function, and pass them the Pose instance. + """ + for h in self.pose_handlers: + h(p) + + def on_arm(self, arm, xdir): + """ + Call each Arm handler function, and pass them the Arm and XDirection instances. + """ + for h in self.arm_handlers: + h(arm, xdir) + + +if __name__ == '__main__': + try: + import pygame + from pygame.locals import * + HAVE_PYGAME = True + except ImportError: + HAVE_PYGAME = False + + if HAVE_PYGAME: + w, h = 1200, 400 + scr = pygame.display.set_mode((w, h)) + + last_vals = None + + def plot(scr, vals): + DRAW_LINES = False + + global last_vals + if last_vals is None: + last_vals = vals + return + + D = 5 + scr.scroll(-D) + scr.fill((0,0,0), (w - D, 0, w, h)) + for i, (u, v) in enumerate(zip(last_vals, vals)): + if DRAW_LINES: + pygame.draw.line(scr, (0,255,0), + (w - D, int(h/8 * (i+1 - u))), + (w, int(h/8 * (i+1 - v)))) + pygame.draw.line(scr, (255,255,255), + (w - D, int(h/8 * (i+1))), + (w, int(h/8 * (i+1)))) + else: + c = int(255 * max(0, min(1, v))) + scr.fill((c, c, c), (w - D, i * h / 8, D, (i + 1) * h / 8 - i * h / 8)); + + pygame.display.flip() + last_vals = vals + + m = MyoRaw(sys.argv[1] if len(sys.argv) >= 2 else None) + + def proc_emg(emg, moving, times=[]): + if HAVE_PYGAME: + # update pygame display + plot(scr, [e / 2000. for e in emg]) + else: + print(emg) + + # print framerate of received data + times.append(time.time()) + if len(times) > 20: + # print((len(times) - 1) / (times[-1] - times[0])) + times.pop(0) + + m.add_emg_handler(proc_emg) + m.connect() + + m.add_arm_handler(lambda arm, xdir: print('arm', arm, 'xdir', xdir)) + m.add_pose_handler(lambda p: print('pose', p)) + + try: + while True: + m.run(1) + + if HAVE_PYGAME: + for ev in pygame.event.get(): + if ev.type == QUIT or (ev.type == KEYDOWN and ev.unicode == 'q'): + raise KeyboardInterrupt() + elif ev.type == KEYDOWN: + if K_1 <= ev.key <= K_3: + m.vibrate(ev.key - K_0) + if K_KP1 <= ev.key <= K_KP3: + m.vibrate(ev.key - K_KP0) + + except KeyboardInterrupt: + pass + finally: + m.disconnect() + print() diff --git a/pyrobolearn/tools/interfaces/sensors/myo_armband.py b/pyrobolearn/tools/interfaces/sensors/myo_armband.py index 77cd155..ab4ced6 100644 --- a/pyrobolearn/tools/interfaces/sensors/myo_armband.py +++ b/pyrobolearn/tools/interfaces/sensors/myo_armband.py @@ -3,26 +3,33 @@ Warnings: "Myo production has officially ended as of Oct 12, 2018 and is no longer available for purchase". -References: - - Myo Armband (webpage for support): https://support.getmyo.com/hc/en-us - - PyoConnect: http://www.fernandocosentino.net/pyoconnect/ - - related useful repository: https://github.com/ijin/venus-lift-lights/blob/master/PyoConnect.py - - MyoLinux: https://github.com/brokenpylons/MyoLinux - - myo-python: https://github.com/NiklasRosenstein/myo-python - - Software for Thalmic's Myo armband: https://github.com/balandinodidonato/MyoToolkit -""" +The Myo Armband has 8 EMG sensors (the sensor with the Myo logo is the 4th sensor, see picture on the web), and +an IMU sensor (which allows to get the orientation, acceleration from the accelerometer, and angular velocity +from the gyroscope). -# TODO: implement this interface +The coordinate system is a right-handed coordinate system, and it is the same as the one used in robotics. +Once the armband is put on your arm and your elbow is bent at 90 degrees, the x-axis is pointing forward, +the y-axis is pointing to the left, and the z-axis is pointing upward. More information can be found on the web. + +This implementation used a slightly modified ``Myo`` class from ``PyoConnect`` from [2], which inherits from +``MyoRaw`` from [3]. + +References: + - [1] Myo Armband (webpage for support): https://support.getmyo.com/hc/en-us + - [2] PyoConnect (Python + Linux): http://www.fernandocosentino.net/pyoconnect/ + - [3] myo-raw (Python + Linux): https://github.com/dzhu/myo-raw +""" import numpy as np try: - import myo + from myo.PyoConnectLib import Myo except ImportError as e: - raise ImportError(repr(e) + '\nTry to install `myo` or `PyoConnect`') + raise ImportError(repr(e) + '\nTry to install `PyoConnect` (version 2), see the `install_myo_ubuntu.txt` file.') from pyrobolearn.tools.interfaces.sensors import SensorInterface +from pyrobolearn.utils.transformation import get_quaternion_from_rpy __author__ = "Brian Delhaisse" @@ -40,41 +47,154 @@ class MyoArmbandInterface(SensorInterface): Warnings: "Myo production has officially ended as of Oct 12, 2018 and is no longer available for purchase". + The Myo Armband has 8 EMG sensors (the sensor with the Myo logo is the 4th sensor, see picture on the web), and + an IMU sensor (which allows to get the orientation, acceleration from the accelerometer, and angular velocity + from the gyroscope). + + The coordinate system is a right-handed coordinate system, and it is the same as the one used in robotics. + Once the armband is put on your arm and your elbow is bent at 90 degrees, the x-axis is pointing forward, + the y-axis is pointing to the left, and the z-axis is pointing upward. More information can be found on the web. + + This implementation used a slightly modified ``Myo`` class from ``PyoConnect`` from [2], which inherits from + ``MyoRaw`` from [3]. + References: - - Myo Armband (webpage for support): https://support.getmyo.com/hc/en-us - - PyoConnect: http://www.fernandocosentino.net/pyoconnect/ - - related useful repository: https://github.com/ijin/venus-lift-lights/blob/master/PyoConnect.py - - MyoLinux: https://github.com/brokenpylons/MyoLinux - - myo-python: https://github.com/NiklasRosenstein/myo-python - - Software for Thalmic's Myo armband: https://github.com/balandinodidonato/MyoToolkit + - [1] Myo Armband (webpage for support): https://support.getmyo.com/hc/en-us + - [2] PyoConnect (Python + Linux): http://www.fernandocosentino.net/pyoconnect/ + - [3] myo-raw (Python + Linux): https://github.com/dzhu/myo-raw """ - def __init__(self, use_thread=False, sleep_dt=0., verbose=False, use_rgb=True, use_depth=True): + def __init__(self, tty=None, use_thread=False, sleep_dt=0., verbose=False, library_verbose=False): """ - Initialize the RealSense input interface. + Initialize the Myo armband input interface. Args: + tty (None, str): TTY (on Linux). You can check the list on a terminal by typing `ls /dev/tty*`. By default, + it will be '/dev/ttyACM0'. use_thread (bool): If True, it will run the interface in a separate thread than the main one. The interface will update its data automatically. sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring or setting the next sample. verbose (bool): If True, it will print information about the state of the interface. This is let to the programmer what he / she wishes to print. + library_verbose (bool): If True, it will print information returned by the library. """ - # TODO + # create myo instance + self.myo = Myo(tty=tty, verbose=library_verbose) + + # connect to myo (this is a blocking call) + self.myo.connect() + + if verbose: + print("Myo Armband is connected!") super(MyoArmbandInterface, self).__init__(use_thread, sleep_dt, verbose) + ############## + # Properties # + ############## + + @property + def roll(self): + """Return the roll angle.""" + return self.myo.current_roll + + @property + def pitch(self): + """Return the pitch angle.""" + return self.myo.current_pitch + + @property + def yaw(self): + """Return the yaw angle.""" + return self.myo.current_yaw + + @property + def rpy(self): + """Return the roll-pitch-yaw angles.""" + return np.array([self.myo.current_roll, self.myo.current_pitch, self.myo.current_yaw]) + + @property + def quaternion(self): + """Return the orientation expressed as a quaternion [x,y,z,w].""" + return get_quaternion_from_rpy(self.rpy) + + @property + def acceleration(self): + """Return the values read by the accelerometer.""" + return np.asarray(self.myo.current_accel) + + @property + def gyro(self): + """Return the values read by the gyroscope.""" + return np.asarray(self.myo.current_gyro) + + @property + def emg(self): + """Return the EMG values returned by the 8 sensors.""" + return np.asarray(self.myo.current_emg_values) + + @property + def current_arm(self): + """Return a string 'left', 'right', or 'unknown' to specify which arm is attached the sensor.""" + return self.myo.current_arm + + @property + def pose(self): + """Return a string describing the last pose (between 'rest', 'fist', 'waveIn', 'waveOut', 'fingersSpread', + 'doubleTap', 'unknown').""" + return self.myo.pose + + @property + def x_direction(self): + """Return a string describing the x direction (between 'unknown', 'towardWrist', and 'towardElbow').""" + return self.myo.current_xdir + ########### # Methods # ########### + def vibrate(self, length): + """Make the myo armband vibrate for length = [1,4].""" + self.myo.vibrate(length) + def run(self): """Run the interface.""" - pass # TODO + ret = self.myo.run() + if ret is None and self.verbose: + print("Connection lost, trying to reconnect.") + self.myo.connect() + + def close(self): + """Close the interface.""" + self.myo.disconnect() + + if self.use_thread: + if self.verbose: + print("Asking for thread to stop...") + self.stop_thread = True # Test the interface if __name__ == '__main__': - pass + import time + + # create the myo interface + myo = MyoArmbandInterface(verbose=True) + + try: + while True: + myo.step() + print("RPY: {}".format(myo.rpy)) + print("Quaternion: {}".format(myo.quaternion)) + print("EMG: {}".format(myo.emg)) + print("Accel: {}".format(myo.acceleration)) + print("Gyro: {}".format(myo.gyro)) + print("") + time.sleep(0.01) + except KeyboardInterrupt: + print("Keyboard Interrupt") + finally: + myo.close() + print("Bye!")