add myo armband interface

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.

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.
+

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<name>.+)$', 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<name>.+)$', 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()

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)

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*

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()

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!")