Tomaszewski, Mark - Thesis: Application of Consumer-Off-The-Shelf (COTS) Devices to Human Motion Analysis

APPLICATION OF CONSUMER-OFF-THE-SHELF (COTS) DEVICES TO

HUMAN MOTION ANALYSIS

by

Mark Tomaszewski

February 2017

A thesis submitted to the

Faculty of the Graduate School of

the University at Buffalo, State University of New York

in partial fulfillment of the requirements for the degree of

Master of Science

Department of Mechanical and Aerospace Engineering

ii

Dedicated to

My family and close friends – for their enthusiastic support and encouragement.

This thesis is a direct product of their love.

iii

Acknowledgements

I must acknowledge a number of people who have had an impact on my

professional, academic, and social growth during my study for this thesis. First, I would like

to thank my advisor, Dr. Venkat Krovi, for offering me an education that goes far beyond the

classroom and the laboratory by incorporating endless opportunities for intellectual,

technical, and professional growth. I would also like to thank Dr. Gary Dargush and Dr. Ehsan

Esfahani for serving as members of my committee. These three professors have collectively

contributed toward the majority of inspiration I have received in my time at university.

I would also like to extend thanks to my colleagues with whom I have shared many

profound experiences as coworkers, labmates, and friends. Thank you to Matthias Schmid

for your guidance and assistance in our shared professional endeavors. Thank you to all of

the members of ARMLAB, both students and interns. In particular, thank you S.K. Jun, Xiaobo

Zhou, Suren Kumar, Ali Alamdari, Javad Sovizi, Yin Chi Chen, and Michael Anson. In some

way, we have all done this together.

iv

Contents

Abstract ................................................................................................................................................................... vii

1 Introduction ................................................................................................................................................... 1

2 Background ................................................................................................................................................. 11

2.1 Myo Overview .................................................................................................................................... 11

2.2 Sphero Overview .............................................................................................................................. 17

3 Software Tools ........................................................................................................................................... 21

3.1 Myo SDK MATLAB MEX Wrapper Development .................................................................. 22

3.2 Sphero API MATLAB SDK Development .................................................................................. 49

3.3 Application Cases .............................................................................................................................. 70

4 Mathematical Methods ........................................................................................................................... 82

4.1 Coordinate Frames, Vectors, and Rotations ........................................................................... 82

4.2 Working with Sensor Data ............................................................................................................ 85

4.3 Upper Limb Kinematics .................................................................................................................. 89

4.4 Calibration Problem ........................................................................................................................ 95

4.5 Experiment Definition .................................................................................................................. 114

5 Motion Analysis ....................................................................................................................................... 118

5.1 Experimental Setup ....................................................................................................................... 118

5.2 Data Collection ................................................................................................................................. 121

5.3 Data Processing and Calibration ............................................................................................... 124

5.4 Analysis Results .............................................................................................................................. 128

6 Discussion .................................................................................................................................................. 133

Appendix A Source Code ......................................................................................................................... 139

References .......................................................................................................................................................... 156

v

Figures

Figure 2-1: Myo teardown disassembly .................................................................................................... 12

Figure 2-2: Myo mainboard (front and back) ......................................................................................... 13

Figure 2-3: Myo APIs and middleware stack ........................................................................................... 16

Figure 2-4: Sphero 2.0 internal electronic and mechanical components .................................... 18

Figure 2-5: Sphero BB-8™ mainboard ....................................................................................................... 19

Figure 2-6: Sphero API and middleware stack ....................................................................................... 20

Figure 3-1: MEX function states, transitions, and actions .................................................................. 37

Figure 3-2: Myo MATLAB class wrapper behavior ............................................................................... 45

Figure 3-3: Sphero API send flowchart...................................................................................................... 62

Figure 3-4: Sphero API receive flowchart ................................................................................................ 66

Figure 3-5: Myo CLI EMG logger plot ......................................................................................................... 71

Figure 3-6: Myo GUI MyoMexGUI_Monitor ......................................................................................... 73

Figure 3-7: Myo GUI MyoDataGUI_Monitor ...................................................................................... 73

Figure 3-8: Sphero CLI gyroscope logger plot ........................................................................................ 75

Figure 3-9: Sphero GUI SpheroGUI_MainControlPanel ......................................................... 77

Figure 3-10: Sphero GUI SpheroGUI_Drive ...................................................................................... 78

Figure 3-11: Sphero GUI SpheroGUI_VisualizeInputData ................................................. 79

Figure 3-12: Myo and Sphero upper limb motion capture ................................................................ 80

Figure 4-1: Upper limb forward kinematics model .............................................................................. 91

Figure 4-2: Calibration point definitions .................................................................................................. 96

Figure 4-3: Calibration point calculated vectors ................................................................................... 97

Figure 4-4: Choice of task space coordinate frame ............................................................................... 97

Figure 4-5: Calibration objective function error vector ...................................................................... 99

Figure 4-6: Experimental analysis plane error vector calculation ............................................... 116

Figure 5-1: Calibration point fixture ........................................................................................................ 118

Figure 5-2: Calibration fixtures assembled onto jig ........................................................................... 119

Figure 5-3: Experimental setup calibration jig and subject ............................................................ 120

Figure 5-4: Data visualization provided by MyoSpheroUpperLimb ....................................... 122

Figure 5-5: Subject performing the t-pose to set the home pose .................................................. 123

Figure 5-6: Calibration data visualization for trial 4 .......................................................................... 129

Figure 5-7: The effect of poor calibration correspondence on plane error............................... 129

Figure 5-8: Magnitude of plane error ep for three reaches in trial 4 ............................................ 130

Figure 5-9: Inverse kinematics joint angle trajectories .................................................................... 131

Figure 5-10: Magnitude of error introduced by inverse kinematics............................................ 132

vi

Tables

Table 1-1: Motion capture systems ................................................................................................................ 4

Table 2-1: Myo SDK offerings ........................................................................................................................ 13

Table 2-2: Myo SDK versus Bluetooth protocol ..................................................................................... 16

Table 2-3: Sphero SDK offerings .................................................................................................................. 19

Table 3-1: MyoData data properties ......................................................................................................... 48

Table 3-2: Sphero API CMD fields ................................................................................................................ 51

Table 3-3: Sphero API RSP fields ................................................................................................................. 52

Table 3-4: Sphero API MSG fields ................................................................................................................ 53

Table 3-5: Command definition for the Ping() command .............................................................. 54

Table 3-6: Response definition for the Ping() command ............................................................... 54

Table 3-7: Command definition for the Roll() command .............................................................. 55

Table 3-8: Response <DATA> definition for the ReadLocator() command ......................... 56

Table 3-9: Response <DATA> interpretation for the ReadLocator() command .............. 56

Table 3-10: Data source MASK bits for SetDataStreaming() command and message .. 57

Table 3-11: Data source MASK for streaming accelerometer data ................................................. 58

Table 3-12: Command parameters for SetDataStreaming command .................................. 58

Table 3-13: Message definition for the DataStreaming message ............................................. 59

Table 3-14: Message <DATA> definition for the DataStreaming message ........................... 60

Table 3-15: Message <DATA> interpretation for the DataStreaming message .................. 60

Table 3-16: Sphero data sources .................................................................................................................. 67

Table 4-1: Inverse kinematics joint variable definitions .................................................................... 94

Table 4-2: Calibration constraints summary ........................................................................................ 109

Table 4-3: Calibration constraint set ........................................................................................................ 111

Table 4-4: Experimental protocol state progression and timing .................................................. 115

Table 5-1: Calibration optimization statistics ....................................................................................... 126

Table 5-2: Calibration subject geometric parameter results .......................................................... 127

vii

Abstract

Human upper limb motion analysis with sensing by way of consumer-off-the-shelf

(COTS) devices presents a rich set of scientific, technological, and practical implementation

challenges. The need for such systems is motivated by the popular trend toward the

development of home based rehabilitative motor therapy systems in which patients perform

therapy alone while a technological solution connects the patient to a therapist by

performing data acquisition, analysis, and the reporting of evaluation results remotely. The

choice to use COTS devices mirrors the reasons why they have become universally accepted

in society in recent times. They are inexpensive, easy to use, manufactured to be deployable

at large scale, and satisfactorily performant for their intended applications. These claims for

the use of COTS devices also resound with requirements that make them suitable for use as

low-cost equipment in academic research.

The focus of this work is on the development of a proof of concept human upper

limb motion capture system using Myo and Sphero. The end-to-end development of the

motion capture system begins with developing the software that is required to interact with

these devices in MATLAB. Each of Myo and Sphero receive a fully-featured device interface

that’s easy to use in native MATLAB m-code. Then, a theoretical framework for upper limb

motion capture and analysis is developed in which the devices’ inertial measurement unit

data is used to determine the pose of a subject’s upper limb. The framework provides

faculties for model calibration, registration of the model with a virtual world, and analysis

methods that enable successful validation of the model’s correctness as well as evaluation of

its accuracy as shown by the concrete example in this work.

1

1 Introduction

Human motion analysis is relevant in the modern day context of quantitative

home-based motor rehabilitation. This motivational application domain frames a rich

landscape within which many challenges exist with respect to the core technology being

leveraged, application-specific requirements, and societal factors that contribute to the

adoption of such systems. The challenges facing the developers of home-based motor

rehabilitation systems come in at least two bulk categories. Perhaps most importantly, there

are application specific requirements that must be met for the solution to be accepted by

practicing professionals. There also exist technological challenges to be overcome, some of

which arise as a result of the previously mentioned application specific challenges.

The scenario which provides an example of the utility inherent in a home-based

motor rehabilitation scheme is one in which the limitations of traditional rehabilitation

therapy are mitigated by the introduction of a technological solution that does not inhibit the

ability of therapists to provide similar quality of patient care. The so-called traditional

rehabilitation scheme typically takes place directly between a therapist and the patient in

the therapist’s office. The fact that patients must travel to receive therapy immediately

constrains the frequency with which they can receive care in most practical situations.

Hence, a first set of challenges is identified as the lessening of the time and distance gap

between patients and the point of care. The provision of care itself is characterized by the

therapist’s human knowledge of the patient’s condition over time. Evaluation of the patient’s

condition is enabled by the therapist’s knowledge and experience in treating patients when

assigning scores to his or her own perception of the patient’s therapy task performance. This

2

application domain knowledge presents a secondary set of application specific challenges to

be overcome with the home-based solution.

The basic technological problem to be addressed in the home-based motor

rehabilitation solution is one in which the desired outcome is a therapist-approved reporting

of the patient’s task performance quality. The development of such metrics is a problem that

is to be considered at a stage when the technological solution provides sufficiently accurate

representation of the subject’s motion. Assuming that this requirement is met, then the

development of motion derived metrics can follow. In addition to providing an accurate

motion representation of the subject, it’s also desirable for the system to provide the

capability for interactivity of the subject with a known task environment containing any

combination of physical or virtual fixtures. This enables monitoring of the subject’s ability to

perform interaction tasks that may be necessary in daily life. The representation of the

subject’s motion as well as the task environment must then also be reliably accurate such as

to be proven through validation testing.

The sensory data acquisition system technology used to support human motion

analysis varies in cost from the order of hundreds of dollars to as much as hundreds of

thousands of dollars. Similar to this gross variation in technology cost, the variety of motion

capture systems also exhibit differing precision, accuracy, and repeatability characteristics.

They also show similar variation in the complexity of setup and calibration procedures which

directly affects the required user skill and the need these systems to be installed in controlled

environments.

A representative range of product offerings that may be used for such human

motion capture systems is shown in Table 1-1 along with indication of the magnitude of cost

3

for each system or device. The top-end system here, made by Vicon, represents the highest

quality motion capture data, but also the highest demands on users and the installation

environment. This optical marker tracking system requires that many cameras be installed

in a rather large room, such as a designated motion analysis laboratory space, and must not

be disturbed for the duration of motion capture activities. A calibration must be performed

in which optical markers are used to “wand” the motion capture volume to perform extrinsic

calibration of the cameras, and the subject must also be instrumented with optical markers

that are affixed to the body precisely on known anatomical landmarks. This system

represents one that is infeasible as a candidate for therapy patients to operate alone at home.

The Motion Shadow motion capture suit uses inertial measurement unit (IMU) sensors to

capture the spatial orientation of the subject’s limbs. Contact based sensor systems such as

this require no environment setup, very few environment requirements, and minimal

complexity to setup the subject-worn apparatus. The only environmental requirement is

minimal presence of electromagnetic interference (EMI) that would introduce errors into

the IMUs’ onboard magnetometer sensor readings. With much less cost (although still

significant) and greater usability for common people, the tradeoff is slightly less fidelity

(lower degree of freedom motion model), precision (data resolution), and accuracy in the

motion representation. This trend is one that continues as we move down the list.

4

Table 1-1: Motion capture systems Images taken from the device manufacturer websites: vicon.com, motionshadow.com, microsoftstore.com, myo.com, and sphero.com

Name Sensing Modality Cost Magnitude Image

Vicon Optical Markers $100,000

Motion Shadow

Wearable IMU (Navigation grade)

$10,000

Kinect Vision (RGB & IR depth)

$100

Myo and Sphero

Wearable IMU (Consumer grade)

$100

One step lower than the Motion Shadow suit, we cross a device accessibility

boundary that makes the remaining devices highly desirable for applications in research.

Perhaps the gold standard in consumer motion capture products is the Microsoft Kinect

sensor. With a price on the order of hundreds of dollars and well developed community

software support for Windows computers as well as the MATLAB environment, this has been

a popular choice for vision based motion capture in academic research applications for many

years. With similar benefits, the Myo gesture control armband, made by Thalmic Labs, and

5

Sphero the robotic ball are runners-up to Microsoft Kinect. In addition to the IMU sensors in

Myo and Sphero, these devices offer other features that make them desirable for use in

motion capture applications for motor rehabilitation.

The Kinect sensor requires virtually no setup and its only requirement on the

environment is direct line of sight to the entire subject during use. The device provides a

representation of human motion that is encoded by the translational positions of the joints

in a skeleton model of the subject. Two limitations on the quality of data received from the

Kinect are due to these two qualities. A pitfall resulting from violation of the line of sight

requirement is that for frames in which there is even partial occlusion of the subject, the

skeleton estimate will either be lost or will fail tragically with an incorrect pose. Such

occlusions can happen also due to self-occlusion of the subject so that certain tasks may not

permissible for capture using the Kinect sensor. Also, the joint position kinematics

description fails to capture the axial rotation of skeleton segments that are parallel to the

image frame. This is not an accident as this is a fundamental limitation in the utility of depth

data for motion capture. A final remark on the limitations of Kinect is that the skeleton

estimation is not subject to any sort of temporal continuity relationship. This means that

higher order motion analysis (for example: velocity and acceleration) of the data must be

performed on data that has been filtered in some way to smooth this noise.

Myo and Sphero bear sensing characteristics that make these devices competitive

options compared to the Kinect due to the fact that they rely on IMU sensor data. As was the

case with Motion Shadow, these devices must only be affixed to the subject in some way. This

is attainable since Myo is designed to be worn on the subject’s arm whereas Sphero is

appropriately sized for the subject to hold it in the hand. In this way, a combination of these

6

devices can be used to capture the pose of a human upper limb. Also, like for Motion Shadow,

the only environmental requirement is minimal EMI. Due to the fact that the IMU sensors for

these devices are of lesser quality than those used in Motion Shadow, we also expect that the

typical gyroscope drift error will be evident in the output data from these sensors due to a

combination of factors that influence sensor calibration errors. Compared to the quality of

Kinect data, the use of both of these devices will not be affected by line of sight occlusion nor

will the kinematic representation fail to identify the orientation of skeleton segments. This

is because the sensors provide their spatial orientation as a data output in the form of a unit

quaternion. Also, the estimated quaternion is the result of an estimation algorithm that, by

its nature, filters and smooths the data. Thus, the kinematic representation from these

devices does not suffer from noise as is the case for Kinect.

In addition to the previously mentioned benefits of Myo and Sphero as IMU data

acquisition devices compared to the vision based Kinect system, we also note other

functionality that is supported by Myo and Sphero because of the intended use for each

device. As a gesture control armband, a particular variation on a natural user interface (NUI)

input device, Myo contains eight surface electromyography (EMG) sensors that are used by

its onboard processor to detect gestures performed by the subject. Myo provides access to

the raw EMG data along with the higher level gesture detection state. This additional sensing

modality is very much relevant to motor rehabilitation, as it may be useful to enhance the

characterization of subject task performance. Sphero the robotic ball is not purposefully built

as an input device although this is a valid secondary use case. Its primary intended

functionality is to be teleoperated as a robotic toy for entertainment purposes. This provides

future work with the opportunity to create hybrid virtual reality environments in which

7

games that exist in both virtual and physical reality can be developed to exercise the

rehabilitation subject in a more immersive way.

These promising attributes of Myo and Sphero motivate the case for using them

in a new and novel way to build an IMU sensor based human upper limb motion capture

system for academic research. In contrast to the software support status for Microsoft Kinect,

these devices have not yet experienced maturing of their support communities. For this

reason, software tools must be developed with which to interact with the devices in a

common environment that’s accessible to a broad spectrum of target users.

Perhaps the first choice to be made in an implementation of software support for

these devices is selection of the end user development environment. In the typical case,

software support is provided for devices in the way of precompiled binary executable

libraries that are linked to the user’s application implementation through code bindings

written in some programming language. In many cases, the programming language here is

chosen to be very general and extensible, such as C++, or otherwise one that is a platform

independent interpreted language such as Python or Javascript. In cases involving lower

level device interfaces specifications, the provided interface may be closer to the physical

communication layer and rely upon the user to implement all supporting application

software. Although this is standard practice in hardware device application software

support, this model assumes that users be proficient with the chosen programming language

in addition to a suitable development environment and tools. For many target end users,

such as undergraduate students, graduate students and academic researchers, and

nontechnical researchers, these assumptions may be prohibitive to working with the

devices.

8

Possible candidates for choice as the integrated development environment (IDE)

that is suitable for the academic and research environment include solutions such as The

MathWorks’ MATLAB and LabVIEW by National Instruments. Both of these environments

provide users with an accessible interface to compiled software along with faculties to test

and debug programmatic solutions and utilities to visualize application data. Although both

of these IDEs could be used, we believe that MATLAB is a more suitable candidate due to its

slightly less structured, and more extensible, program development capabilities. Since

LabVIEW is primarily a graphical programming environment intended for use in data

capture and visualization, it may not provide the best possible environment in which to

interface devices requiring time evolving changes to control state.

We can also look to the past success of other comparable devices with existing

support for MATLAB to gain some insight. For example, there exist publicly available projects

for Microsoft’s Kinect V1 [1] and Kinect V2 [2]. The reach of these projects to the MATLAB

user community is evidenced by average download rates of 200-400 downloads per month

and average ratings of 4.8 out of 5 stars. The use benefits of software support such as these

packages is stated quite well by the developers of this Kinect V2 package. According to Tervin

and Córdova-Esparza there is a tradeoff between implementations in MATLAB compared to

those using native code with the Kinect software development kit (SDK) in the way of 30%

performance degradation and with an order of magnitude in code size reduction [3].

In addition to the utility and reach of the software solution, the implementation

should also be correct as well as conformant to the implementation of the underlying

interface without obscuring the device capabilities from the user. Other interfaces to Myo

and Sphero exist for the MATLAB environment, but in various ways each of these fails to

9

adhere to these requirements. In [4], Boyali and Hashimoto utilize the matMYO project [5]

to acquire data from Myo for their application in gesture classification research. This

implementation is used only for the batch recording of data for future offline analysis. The

data is not made available to the user until recording has completed, thus greatly limiting the

capability of the interface to be used for interactive applications. This implementation also

assumes that the collected dataset is synchronized and complete with no missing samples

without the performing any validity checks. Before the start of this work, the Sphero

MATLAB Interface [6] was made available by Lee to provide users with the capability to send

a handful of synchronous commands to Sphero in order to move it within its environment

and read odometry data, for example. Within one week of the release of the code developed

for Sphero in this work, The MathWorks released the Sphero Connectivity Package [7]

created by Sethi which provides users with a slightly more complete set of device features.

Both of these alternate interfaces to Sphero obscure the user from the full capability of

Sphero through software abstraction and lack of implemented features.

The first half of this work focuses on the development of software interfaces to

Myo and Sphero in the MATLAB environment. We set out to achieve the success shown in

the community use of the MATLAB packages for Microsoft Kinect while correctly

representing the available device data and state to the user in a way that’s consistent with

the intended functionality of the device. Although code performance may be suboptimal in

these MATLAB implementations, we realize that the intention of this exercise is to broaden

the spectrum of users who will benefit from programmatic accessibility to these devices.

More importantly, we intend to reduce code size and complexity for user applications while

simplifying the programmatic code path to device data.

10

Through the development of software tools, we explore and begin to understand

more greatly the ways in which the device data can be connected to analysis algorithms to

obtain kinematic representation of the physical world. Development of application case

examples for the Myo and Sphero software tools leads to a combined (Myo and Sphero)

application of human upper limb motion analysis which serves as the so-called “zeroth-

order” approximation to the rest of the motion capture modeling and analysis in this work.

The remainder of this thesis is organized with the following structure. In section

2, Background, we cover the necessary prerequisite information on the Myo and Sphero

devices. In section 3, Software Tools, we develop the open source interface software that

enables academic researchers to use all device features relevant to engineering research

with minimal effort in MATLAB. Then in section 4, Mathematical Methods, we develop the

mathematical framework that is used to implement upper limb motion capture using two

Myo devices and one Sphero using the software tools developed in the previous section.

Section 5, Motion Analysis, documents the implementation of the motion analysis scheme

and presents the results that will allow us to validate the effectiveness of the complete

system. Finally, in section 6, Discussion, we discuss the results with respect to the software

tool development and the mathematical methods before closing with suggestions of rich

areas for future work.

Videos that depict the intermediate results of this work can be found at this

YouTube channel: https://www.youtube.com/channel/UCnrXD_jBuv_P14kC7isMBeQ.

Notable contributions to this video repository include demonstrations of the software tools

as well as visualizations of the virtual representations of the human upper limb generated in

the course of this work.

https://www.youtube.com/channel/UCnrXD_jBuv_P14kC7isMBeQ

11

2 Background

In this section we present an introduction to the core technology for the devices

that we are utilizing in this work. Each of Myo and Sphero will be introduced in terms of their

hardware capabilities and application programming interface (API) software support in

preparation for the development of their middleware software interfaces in the following

section.

2.1 Myo Overview

Thalmic Labs’ Myo gesture control armband is a consumer product that is

marketed for use as a NUI input device for general purpose computers. The core technology

empowering its NUI capability is representative of the modern state of the art in sensing

technology. The main features of Myo include the ability to control applications based upon

high-level outputs in the form of the device’s spatial orientation and the detection of gestures

performed by the user. These outputs are derived on-board Myo from raw data that is

measured by way of an IMU and eight EMG sensors, respectively.

2.1.1 Myo Hardware

An in depth look at the underlying hardware of Myo is found in the documentation

of a device teardown that was performed by a popular hobby electronics company named

Adafruit Industries [8]. Although the marketing materials give some indication of the

expected hardware inside Myo, these pictures of the actual components populating the

inside of its enclosure provide proof of the technology Myo relies upon.

Figure 2-1 shows a series of photos from the teardown article that illustrate the

physical constitution of the device. Here we see the main EMG sensor hardware built into the

12

inside of the pods that make up the armband with one of the pods reserved to hold the device

mainboard and batteries. The mainboard contains the remainder of the device hardware that

interests us except for operational amplifiers attached to each of the EMG sensors (not

shown here).

Figure 2-1: Myo teardown disassembly

The mainboard for Myo, shown in Figure 2-2, houses its microcontroller unit

(MCU), IMU sensor, and Bluetooth Low Energy (BLE) module. Also located in the same pod

is a vibration motor attached to the battery board. The Freescale Kinetis M series MCU

contains a 32 bit ARM architecture 72MHz Cortex M4 CPU core with floating point unit

hardware. This particular series of MCU is targets low power metrology applications. The

BLE module enables external communication between Myo and a client computer. The IMU

chip made by Invensense is a 9 axis model containing an onboard digital motion processor

(DMP) which performs sensor fusion on the raw sensor data. The MPU-9150 contains a 3

axis magnetometer, 3 axis gyroscope, and 3 axis accelerometer all in the same silicon die.

The DMP fuses these raw data sources using a proprietary undocumented algorithm to

produce an estimated quaternion. All data outputs, raw and calculated, are made available

13

through a first-in-first-out (FIFO) buffer that is read by the MCU over either a serial

peripheral interface (SPI) or inter-integrated circuit (IIC or I2C) communication bus.

Figure 2-2: Myo mainboard (front and back)

2.1.2 Myo Software

Thalmic Labs has created a rather large ecosystem for development of Myo

enabled applications. The company released officially supported SDKs for four compute

platforms, both desktop and mobile. Thalmic also fosters a larger community of developers

who have contributed projects for Myo in a variety of programming languages. Table 2-1

contains a non-exhaustive list of these offerings to show the diversity of the development

ecosystem surrounding Myo.

Table 2-1: Myo SDK offerings A listing of Myo SDK offerings from official [9] and community [10] sources.

Operating System Language Dependencies Supported By

Windows C++ Myo SDK runtime library

Thalmic Labs

Mac OS X C++ Myo SDK framework

Thalmic Labs

iOS Objective-C MyoKit framework

Thalmic Labs

Android Java Java Library Thalmic Labs

14


Windows C#, .NET --- Community

Linux C, C++, Python --- Community

Mac OS X Objective-C --- Community

--- Unity, Python, Javascript, Ruby, Go, Haskell, Processing, Delphi, ROS, Arduino, MATLAB

--- Community

In addition to these available existing software projects, Thalmic Labs has also

completely opened developer accessibility to Myo by publicly providing the specification for

its physical communication modality in the form of a BLE Generic Attribute (GATT) Profile

specification [11]. The provision of this resource allows developers to completely bypass all

compute platform and programming language dependencies by developing strictly for the

physical BLE communication itself. This is what has enabled creation of the community

projects for Linux and Arduino indicated in Table 2-1, and it also provides future

developments with a powerful option to leverage toward their own projects.

The main advantage when considering developing against the BLE protocol for

Myo is the fact that every implementation detail of the solution can be specified as desired.

These details include not only those concerning the software architecture, but also the

absence of some inherently limiting choices that might be made in other higher level

software solutions such as Myo SDK. One example of this is the fact that the combination of

Myo SDK and Myo Connect limit the use of only one dongle per instance of Myo Connect per

system. The effect of this limitation is that multiple Myo devices must share a single dongle

15

if desired to be used on the same machine. Consequently, not all EMG data can be received

due to hardware limitations in the throughput capacity of the provided BLE dongle.

Although the development freedom of leveraging the BLE specification directly

may be appealing, this option comes with a nontrivial cost. Along with the freedom to specify

every software and hardware choice related to Myo communication and control comes the

responsibility of making the best decisions and the need to compose solutions for all of them.

Some of the decisions that would need to be made involve the choice of supporting hardware

such as Bluetooth radios along with a suitably stable and deployable BLE software stack. All

of this low-level development must first be performed before then working on the layers

which may otherwise be occupied by the officially supported Myo SDK from Thalmic Labs.

The layout of the software stack being described here is presented in Figure 2-3.

We can envision the possible paths between the Myo Device (bottom right) and our intended

MATLAB Interface (top left). Development against the Myo SDK leverages the Myo Connect

desktop application and Myo SDK runtime library with C++ bindings running on the

Application Computer as well as the included BLE dongle hardware. The entry point for

developers into the Myo SDK stack is in their implementation of the Myo SDK C++ API. It’s

from this location in the stack that we compare to the similar level in the low-level API

middleware that targets the BLE specification.

16

Figure 2-3: Myo APIs and middleware stack

A summary of the advantages and disadvantages that are active in our choice

between developing with Myo SDK versus the BLE protocol is collected in Table 2-2. Due to

the level of complexity and sheer volume of code involved in developing from the BLE GATT

specification, the continued active support the Thalmic Labs provides for Myo SDK, and

acceptance of the tradeoff that we will not be able to leverage the EMG data when working

with multiple Myo devices, we choose to use the Myo SDK in this work.

Table 2-2: Myo SDK versus Bluetooth protocol

Advantages Disadvantages

Myo SDK Vendor support

Hardware included with Myo

No EMG data with multiple Myo devices

BLE Protocol

Free choice for all hardware and software

Code volume and complexity

Not as easily deployable

17

2.2 Sphero Overview

Sphero has undergone two major revisions since its first appearance in consumer

markets in the year 2011. The original Sphero received an incremental redesign and the

official name was changed to “Sphero 2.0” in 2014. Although we choose to drop the version

number when referring to Sphero in this work, Sphero 2.0 is the model we are working with

here. The device then received a facelift along with changes to its Bluetooth communication

technology with the release of a new Star Wars™ themed product named BB-8™ in 2015.

Aside from a change from Bluetooth Classic to Bluetooth Low Energy from Sphero 2.0 to BB-

8™, it appears that similar hardware is used in both devices according to a technical blogger

on the Element 14 Community engineering community web platform [12]. In this section we

begin by looking at the hardware inside Sphero devices followed by a survey of the developer

software support.

2.2.1 Sphero Hardware

The first thing we notice when attempting to take a look inside Sphero is its solid

water proof plastic shell. The first step of disassembling this device, shown in Figure 2-4, is

to mechanically split the robotic ball’s shell by cutting (right). Then, the view inside of the

device reveals a two wheeled inverted pendulum type of vehicle that drives around the

inside of the spherical shell in a manner similar to that in which a hamster will run inside its

wheel. The two wheels are powered through gear reduction by two direct current (DC)

motors. Contact traction between the wheels and the shell is maintained by force provided

by a spring in the top mast of Sphero’s chassis. And finally, the main feature of this internal

assembly that we’re interested in is the mainboard containing the interface electronics and

sensors.

18

Figure 2-4: Sphero 2.0 internal electronic and mechanical components

Image source: http://atmega32-avr.com/wp-content/uploads/2013/02/Sphero.jpg

As mentioned previously, public information about the specific components used

in Sphero 2.0 is hard to come by, so we base further inspection on a teardown of BB-8™ with

its mainboard shown in Figure 2-5. This mainboard houses many components, of which

three are particularly important. The Toshiba TB6552FNG dual DC motor driver provides

power to the motors via pulse width modulation (PWM) while offering an electromagnetic

field (EMF) intensity signal to provide feedback for closed loop control. A 6 axis Bosch

BMI055 IMU provides 3 axis gyroscope and 3 axis accelerometer data in a low power

package. In contrast to the MPU-9150 used by Myo, this IMU does not contain a

magnetometer reference, and it also does not perform any digital signal processing on board.

Rather, the ST Microelectronics MCU must read the IMU and provide sensor fusion and

quaternion estimation features. The STM32F3 contains a 32 bit ARM architecture 72MHz

Cortex M4 CPU core with a built in floating point unit just like the Freescale MCU used in

Myo.

http://atmega32-avr.com/wp-content/uploads/2013/02/Sphero.jpg

19

Figure 2-5: Sphero BB-8™ mainboard

2.2.2 Sphero Software

Similar to the API support provided by Thalmic Labs for Myo, Sphero has provided

both platform specific SDKs as well as a specification for Sphero’s low-level serial binary

Bluetooth communication API. In the case of Sphero, the officially supported platform

specific SDKs primarily target mobile computing platforms such as Android and iOS as

shown in Table 2-3.

Table 2-3: Sphero SDK offerings A listing of manufacturer and community supported interfaces to Sphero [13]


iOS Objective-C RobotKit SDK framework

Sphero

iOS Swift RobotKit SDK framework

Sphero

Android Java RobotLibrary SDK jar library

Sphero

--- Javascript Source code Community

These options are less desirable for the intended application in this project since

we prefer to build applications on typical general purpose compute platforms such as Mac,

20

Linux, or Windows. In this case, we choose to take a closer look at the prospect of developing

for the low-level binary protocol as the preferred candidate API.

Figure 2-6: Sphero API and middleware stack

Sphero’s low-level binary protocol, shown in the software stack diagram of Figure

2-6, is a serial communication protocol that is transmitted over its Bluetooth Classic physical

network connection with the client computer. The particular profile employed in this

Bluetooth Classic communication channel is named the serial port profile (SPP). The matter

of implementing Bluetooth communications in a client application is typically addressed in

user space by the implementation of system (or third party) libraries, but in the case of

Bluetooth Classic SPP in MATLAB, this is not the case. The Instrument Control Toolbox of

MATLAB has extended support to Bluetooth devices that implement SPP specifically. What

this means is that MATLAB provides platform independent read and write functionality to

the required Bluetooth device for communication with Sphero in native m-code. Since it is

possible to write native MATLAB m-code that communicates directly to Sphero, this is the

preferred choice for Sphero development API in this work.

21

3 Software Tools

Although the present work demands the creation of MATLAB interfaces for Myo

and Sphero, the broad objective is farther reaching. We also aim to provide academic

students and researchers with a simple way to interact with the devices and their data

programmatically. Furthermore, having chosen MATLAB as the development platform also

enables a larger research based workflow in a development environment that is familiar to

many target users. The following list captures the statement of the guidelines that are

followed throughout the development of these software tools.

All features should be accessed with MATLAB m-code

Common tasks should be wrapped in utility functions

Operations critical to functionality should be performed automatically

The device should be brought live and functional with a single function call

Device data should be automatically stored in the workspace

All features should be well documented

The satisfaction of these guidelines for creating the user-facing MATLAB code

should become evident throughout the remainder of this section. In addition to devising

strategies through which to present the user with a MATLAB m-code interface, we must also

connect the m-code to the physical hardware in some appropriate manner. The presentation

of the code required to achieve these goals will include explanation of the core principles and

logic of the solution followed by detailed explanation of the implementation code. This

section is intended to fully document the function of the software tools for Myo and Sphero

so as to serve as a reference to users of the resulting code base for both functional and

educational purposes.

22

In the remainder of this section we will first discuss the bottom-up development

of Myo SDK MATLAB MEX Wrapper [14] and Sphero API MATLAB SDK [15]. Each of these

subsections begins with a discussion of the basic concepts for the chosen API. Then we

develop the layers of code needed to bridge device communication and control with the user-

facing MATLAB m-code interface described above. Finally, we showcase some individual and

some combined application cases for these devices to provide perspective on the user

experience with respect to the guidelines above resulting from these software tools.

3.1 Myo SDK MATLAB MEX Wrapper Development

In this section, we progress through the design documentation for this interface.

We begin by introducing the basic concepts of the API chosen in section 2.1.2, the Myo SDK

API. Then we follow the path up the software stack through discussing the core aspects of

implementing Myo SDK, development of a MATLAB EXternal (MEX) interface to the Myo

SDK, and finally the design of two MATLAB classes to manage the state of the MEX interface

and the Myo device data for Myo SDK MATLAB MEX Wrapper [14], [16].

3.1.1 Myo SDK Concepts

Every Myo SDK project depends upon the use of the Myo Connect application. The

first step toward interacting with the device involves the end user connecting to the physical

Myo device using the Myo Connect desktop application and the included BLE dongle. Once

connected, the API provided by Myo SDK will enable third party code to interact with the

device by calling into Myo Connect through a runtime library.

The API provided by Myo SDK is in the form of C++ bindings to a dynamically

linked library that calls into Myo Connect. The API handles data communication with an

23

event driven paradigm. Our main objective, data acquisition, is implemented through use of

the SDK by way of user defined implementations of virtual functions of a Myo SDK

application class, myo::DeviceListener. These functions serve as callbacks for events

that provide data from Myo. Additionally, the Myo SDK provides functions that can be

thought of as service calls to change device configuration and state. The definition of these

virtual functions along with the class to which they are members is the core foundation of an

implementation of Myo SDK.

3.1.2 Myo SDK Implementation

The Myo SDK C++ bindings receive events from Myo through the virtual functions

of a user defined application class that inherits from myo::DeviceListener and is

registered to the myo::Hub as a listener. The developer uses an instance of myo::Hub to

invoke the event callbacks by calling myo::Hub::runOnce() periodically. Some Myo

services can be called through members of myo::Myo, but the data acquisition functionality

we desire depends mostly upon the implementation of our myo::DeviceListener

derived application class and its interaction with another application class named MyoData

that manages queued and synchronized streaming data from each Myo device [17]. In this

section, we’ll step through our Myo SDK implementation from its highest level of use and

systematically drill down to the underlying business logic when appropriate.

The initialization of Myo SDK should be preceded by awakening of all Myo devices

that are connected to the host computer via the Myo Connect application. Then, a session is

opened by instantiation of a myo::Hub*.

myo::Hub* pHub;

pHub = new myo::Hub("com.mark-toma.myo_mex");

24

if ( !pHub )

// ERROR CONDITION

The instantiation of pHub invokes the Myo SDK C-API to communicate with the

Myo Connect process, and is identified by a globally unique application identifier string such

as “com.mark-toma.myo_mex”. If pHub is NULL then there was an unrecoverable error

in communicating with Myo Connect, and the program should terminate. Otherwise, the next

step is to validate the existence of a Myo device in the Myo Connect environment by calling

myo::Hub::waitForMyo().

myo::Myo* pMyo;

pMyo = pHub->waitForMyo(5);

if ( !pMyo )

// ERROR CONDITION

In similar fashion, a resulting NULL value for pMyo indicates the failure of Myo

Connect to validate the existence of a connected Myo device. In this case, the program should

terminate. Otherwise, we know that at least one Myo device is connected to Myo Connect.

Now that the connectivity is established, we create our application class and register it as a

listener to pHub so that we can begin to receive events from Myo Connect.

// unsigned int countMyosRequired = <user-specified integer>

DataCollector collector;

if (countMyosRequired==1)

collector.addEmgEnabled = true;

pHub->addListener(&collector);

The DataCollector class inherits from myo::DeviceListener so we are

now ready to receive callbacks in collector from Myo Connect. Immediately following

instantiation of collector, we configure EMG streaming if the expected number of Myo

devices is exactly one. Then, in order to allow for callbacks to be triggered for some

duration in milliseconds, we invoke myo::Hub::run(duration).

25

#define INIT_DELAY 1000 // milliseconds

// unsigned int countMyosRequired = <user-specified integer>

pHub->run(INIT_DELAY);

if (countMyosRequired!=collector.getCountMyos())

// ERROR CONDITION

Here, we are introduced to our first public member function of DataCollector.

The function DataCollector::getCountMyos() returns the number of unique Myo

devices that have been identified in collector as a result of having been passed from Myo

Connect through callback functions. If this number is different than the number of Myos the

user has specified to the application, then the program should terminate. Otherwise, the

program should begin to continually call myo::Hub::runOnce() in a separate thread so

that all callbacks are triggered. At this point, the collector should be configured to

initialize its data logs for Myo.

collector.syncDataSources();

collector.addDataEnabled = true;

Whenever DataCollector::addDataEnabled is toggled from false to

true, the data sources must be synchronized. Since this flag allows the data callbacks to fall

through when unset, the data logs will be in an unknown state. Thus, when toggling the flag

to true, we must synchronize the data queues by calling

DataCollector::syncDataSources(). This function pops all previously logged data

from the data logs in collector.

Finally, the last remaining function to be performed publicly on the

DataCollector is the reading of the data log queues. For this purpose, we have two

struct objects that represent the data sampled from a single instant in time. Since we

sample the data at two different rates, we use two distinct data frames. The data elements

that are always available are sampled at 50Hz and reported in FrameIMU objects.

26

struct FrameIMU

{

myo::Quaternion<float> quat;

myo::Vector3<float> gyro;

myo::Vector3<float> accel;

myo::Pose pose;

myo::Arm arm;

myo::XDirection xDir;

};

When EMG streaming is enabled during use of only a single Myo, we will also work

with FrameEMG objects.

struct FrameEMG

{

std::array<int8_t,8> emg;

};

The data is read from collector by popping the oldest available data frame

from the queues in collector using the functions DataCollector::getFrameXXX().

The following code example shows how this may be performed in the case that either one or

two Myos are being used. In the event that three or more Myos are being used, this example

can be adapted by reading only FrameIMU from each additional Myo device.

// Declarations and initializations

unsigned int iiIMU1=0, iiEMG1=0, iiIMU2=0, iiEMG2=0;

unsigned int szIMU1=0, szEMG1=0, szIMU2=0, szEMG2=0;

FrameIMU frameIMU1, frameIMU2;

FrameEMG frameEMG1, frameEMG2;

unsigned int countMyos = collector.getCountMyos();

if (countMyos<1) /* ERROR CONDITION */;

#define READ_BUFFER 2 // number of samples to leave in collector

szIMU1 = collector.getCountIMU(1)-READ_BUFFER;

if (countMyos==1) {

szEMG1 = collector.getCountEMG(1)-READ_BUFFER;

} else if (countMyos==2) {


} // else if (countMyos==N) // optionally extend to handle more Myos

// --- AQUIRE LOCK ON myo::Hub::runOnce() ----------------------------

// --- BEGIN CRITICAL SECTION ----------------------------------------

while (iiIMU1<szIMU1) { // Read from Myo 1 IMU

frameIMU1 = collector.getFrameIMU(1);

// process frameIMU1

iiIMU1++;

27

}

while (iiEMG1<szEMG1) { // Read from Myo 1 EMG

frameEMG1 = collector.getFrameEMG(1);

// process frameEMG1

iiEMG1++;

}



// process frameIMU2

iiIMU2++;

}



// process frameEMG2

iiEMG2++;

}

// --- BEGIN CRITICAL SECTION ----------------------------------------

// --- RELEASE LOCK --------------------------------------------------

We also note that the calls into DataCollector::getFrameXXX() must be

performed when holding a lock against the thread that is triggering callbacks by invoking

myo::Hub::runOnce() to avoid corruption of data queue synchronization.

Up to this point, we have seen a high-level view of the Myo SDK implementation

without much mention of the underlying implementation details. In the remainder of this

section, we will use this high level blueprint as a framework for describing the functionality

of the DataCollector application class as well as the MyoData data management class.

The DataCollector class is our lowest interface to the Myo device data as it is the

subscriber to streaming device data, and MyoData is a helper class whose objects are owned

by DataCollector and store this data in synchronized FIFO queues. The complete

implementation of the file containing DataCollector and MyoData, myo_class.hpp,

can be found in Appendix A.1 for reference.

The DataCollector class inherits from myo::DeviceListener which is

defined with several virtual methods that are used as callbacks by pHub when Myo

streaming data and state change events occur. It also owns pointers to MyoData objects that

28

are stored in a private member variable std::vector<MyoData*> knownMyos.

Perhaps most importantly, it defines public member functions that are used to control the

behavior of DataCollector and its MyoData instances as well as read the logged data

stored in knownMyos. Finally, for completeness, we note that DataCollector also defines

some member functions and variables for utility. The following is a representation of the

class declarations with some input parameter lists partially omitted for brevity.

class DataCollector : public myo::DeviceListener

{

std::vector<MyoData*> knownMyos;

public:

// Properties

bool addDataEnabled; // onXXXData() falls through when unset

bool addEmgEnabled; // onEmgData() falls through when unset

// Construction, deletion, and utility

DataCollector();

~DataCollector();

void syncDataSources();

const unsigned int getMyoID(myo::Myo* myo,uint64_t timestamp);

// Accessors

unsigned int getCountIMU(int id);

unsigned int getCountEMG(int id);

const FrameIMU &getFrameIMU( int id );

const FrameEMG &getFrameEMG( int id );

const unsigned int getCountMyos();

// State change callbacks

void onPair(myo::Myo* myo, uint64_t timestamp,...);

void onUnpair(myo::Myo* myo, uint64_t timestamp,...);

void onConnect(myo::Myo *myo, uint64_t timestamp,...);

void onDisconnect(myo::Myo* myo, uint64_t timestamp);

void onLock(myo::Myo* myo, uint64_t timestamp);

void onUnlock(myo::Myo* myo, uint64_t timestamp);

void onArmSync(myo::Myo* myo, uint64_t timestamp,...);

void onArmUnsync(myo::Myo* myo, uint64_t timestamp);

// Data streaming callbacks

void onOrientationData(myo::Myo* myo, uint64_t timestamp,...);

void onGyroscopeData (myo::Myo* myo, uint64_t timestamp,...);

void onAccelerometerData (myo::Myo* myo, uint64_t timestamp,...);

void onEmgData(myo::Myo* myo, uint64_t timestamp,...);

void onPose(myo::Myo* myo, uint64_t timestamp,...);

}; // DataCollector

The instantiation of DataCollector is assumed to use the default constructor

which creates and instance with both addDataEnabled and addEmgEnabled assigned

the value false. The destructor function is also quite simple in that it iterates through

29

knownMyos to delete all instances. Immediately following instantiation of collector, we

first set the addEmgEnabled member before then registering it as a listener to pHub. Next,

we run pHub for a brief initialization period to allow callbacks to run from all available Myo

devices so that available Myos can be detected by collector. It is in this process that the

automatic event based behavior inherent in DataCollector begins with the instantiation

of MyoData objects in knownMyos.

The first operation performed in each data streaming callback as well as the

onConnect() and onPair() callbacks is to access the internal identifier for the Myo

producing the event, myo::Myo* myo, by calling

DataCollector::getMyoID(myo,...). This member function returns the index of

myo in knownMyos, but its implicit functionality is to push myo onto knownMyos if it

doesn’t already belong. In this way, the first time a callback is triggered from a Myo device

such that the device will be available in the future, the corresponding MyoData instance is

created in knownMyos.

After the brief initialization delay duration of the previous call to pHub->run(),

we can use DataCollector::getCountMyos() to access the length of the knownMyos

vector. Since it’s assumed that all Myos available in Myo Connect will fire callbacks during

the initialization period, we can terminate the program if the number of Myos expected

doesn’t match the number of Myos returned by this function call.

At this time, we will ensure that collector and its MyoData instances are

properly initialized to a known data state by calling

DataCollector::syncDataSources() to remove any previously logged data. Then

30

we’re ready to set DataCollector::addDataEnabled to true so that onXXXData()

callbacks will pass the received data into the corresponding MyoData instance in

knownMyos.

The only remaining function to be performed on collector is the reading of

logged data by use of the getFrameXXX(id) member functions. These functions are

simply wrappers for similarly named data accessors in the MyoData class. Now, we’ll move

on to look at the data management functionality in MyoData to complete the description of

this Myo SDK implementation.

The primary function to be performed by the MyoData class is to manage the

streaming data that is provided and consumed by DataCollector. This class must receive

individual samples of data from various sources in any sequence via the use of the member

functions onXXXData(). Then, since some data might be lost in transmission, it must

provide automated functionality to synchronize multiple streams that are sampled on the

same time base such as the quaternion, gyroscope, and accelerometer data. This is

performed by the syncXXX() functions. Then the data is consumed, oldest synchronized

sample first, by calling the public getFrameXXX() functions.

Most of the data associated with the MyoData class is stored internally with

private member variables. Only some information such as the Myo device’s myo::Myo*

pointer, the current number of logged data frames, and the data frames themselves are

available externally. The raw streaming data is stored in

std::queue<T,std::deque<T>> double-ended queue containers with type T

corresponding to the datatype used in Myo SDK. All other member variables encode data

stream state that is necessary for use in the business logic of queue synchronization.

31

The class declaration for MyoData is shown here with some input parameter lists

and the std::queue<> type names partially omitted for presentation clarity.

class MyoData

{

// Properties

myo::Myo* pMyo;

FrameIMU frameIMU;

FrameEMG frameEMG;

bool addEmgEnabled;

// Streaming data queues

std::queue<myo::Quaternion<float>,std::deque<...>> quat;

std::queue<myo::Vector3<float>,std::deque<...>> gyro;

std::queue<myo::Vector3<float>,std::deque<...>> accel;

std::queue<myo::Pose,std::deque<myo::Pose>> pose;

std::queue<myo::Arm,std::deque<myo::Arm>> arm;

std::queue<myo::XDirection,std::deque<myo::XDirection>> xDir;

std::queue<std::array<int8_t,8>,std::deque<...>> emg;

// Streaming data state

uint64_t timestampIMU;

unsigned int countIMU;

unsigned int semEMG;

unsigned int countEMG;

uint64_t timestampEMG;

// Construction, deletion, and utility

MyoData(myo::Myo* myo, uint64_t timestamp, bool _addEmgEnabled);

~MyoData();

void syncIMU(uint64_t ts);

bool syncPose(uint64_t ts);

bool syncArm(uint64_t ts);

bool syncXDir(uint64_t ts);

void syncEMG(uint64_t ts);

void syncDataSources();

// Accessors

myo::Myo* getInstance();

unsigned int getCountIMU();

unsigned int getCountEMG();

FrameIMU &getFrameIMU();

FrameEMG &getFrameEMG();

// Add data

void addQuat(const myo::Quaternion<float>& _quat,...);

void addGyro(const myo::Vector3<float>& _gyro,...);

void addAccel(const myo::Vector3<float>& _accel,...);

void addEmg(const int8_t *_emg,...);

void addPose(myo::Pose _pose,...);

void addArm(myo::Arm _arm,...);

void addXDir(myo::XDirection _xDir,...);

}; // MyoData

The constructor for MyoData requires a pointer to myo::Myo*, a 64-bit

timestamp, and a configuration flag indicating the behavior for streaming EMG data as shown

32

by its signature above. When called by DataCollector, the addEmgEnabled flag is

passed from its corresponding member variable whereas the other parameters are passed

through from the generating callback function. The operations performed by the constructor

are to initialize the Myo device and then initialize the class properties by filling the data log

queues each with a dummy sample and setting streaming data state accordingly. Myo device

initialization includes calling the unlock service to force Myo into a state that allows data

streaming as well as enabling EMG data streaming from the device if applicable. The

following is an abbreviated representation of the constructor implementation.

MyoData(myo::Myo* myo, uint64_t timestamp, bool _addEmgEnabled)

: countIMU(1), countEMG(1), semEMG(0), timestampIMU(0), timestampEMG(0)

{

pMyo = myo;

pMyo->unlock(myo::Myo::unlockHold);

if (_addEmgEnabled) {

pMyo->setStreamEmg(myo::Myo::streamEmgEnabled);

// INITIALIZE EMG QUEUE AND STATE

}

addEmgEnabled = _addEmgEnabled;

// INITIALIZE IMU QUEUE AND STATE

}

Once a MyoData object has been created, then data can be added by calling its

addXXXData() functions. These functions populate the associated data queue with the

new data, check data synchronization status, and handle synchronization operations if

necessary. This is performed by collector on its MyoData vector knownMyos as

depicted by the example of adding accelerometer data which is sampled on the same time

base as quaternion and gyroscope data.

The onAccelerometerData callback of DataCollector shown here first

checks the addDataEnabled property and falls through if unset. Otherwise, the function

33

continues to provide the new accelerometer data to the appropriate MyoData object in

knownMyos.

void DataCollector::onAccelerometerData (myo::Myo* myo,

uint64_t timestamp, const myo::Vector3<float>& a)

{

if (!addDataEnabled) { return; }

knownMyos[getMyoID(myo,timestamp)-1]->addAccel(a,timestamp);

}

First, getMyoID() is called to return the one-based index of the provided

myo::Myo* in knownMyos. Then zero-based indexing is used to access the corresponding

MyoData object. The appropriate add data function, addAccel() in this case, is called on

this MyoData instance to pass the new data through.

The addAccel() function first checks that all data on the appropriate time base

is currently synchronized by calling syncIMU() before pushing the new accelerometer data

onto its queue as shown in the following.

void addAccel(const myo::Vector3<float>& _accel, uint64_t timestamp)

{

syncIMU(timestamp);

accel.push(_accel);

}

In the case of the IMU data, synchronization checks for data integrity by using the

timestamp and checking the lengths of the IMU data queues. If the current timestamp is

greater than the previously stored timestamp, then the new data is for a more recent sample.

In this case, the lengths of the IMU data queues should be the same. Otherwise, a

synchronization failure is detected, and this soft failure is recovered by zero-order-hold

interpolation of all the short queues.

The other add data functions follow very similar logic except that the synchronize

functions vary for syncEMG(), syncPose(), syncArm(), and syncXDir(). Since EMG

34

data is provided two samples per timestamp, data corruption is identified when less than

two samples are logged before a new timestamp is received. And since the pose, xDir, and

arm data are only provided as events, we interpolate them on the IMU time base with zero-

order-hold when a new value is received.

Data is read from the MyoData queues by using the public member functions

getFrameXXX() with similar names to these functions of DataCollector. The

implementations of these functions are shown below.

// DataCollector

const FrameIMU &getFrameIMU( int id )

{

return knownMyos[id-1]->getFrameIMU();

}

// MyoData

FrameEMG &getFrameEMG()

{

countEMG = countEMG - 1;

frameEMG.emg = emg.front();

emg.pop();

return frameEMG;

}

The wrapper for the accessor in DataCollector uses the provided one-based

index id to index into knownMyos and call the similar accessor function on this MyoData

instance. The accessor in MyoData simply constructs the FrameXXX structure by reading

and popping the data elements off of their queues.

As mentioned earlier when first introducing data reading using

DataCollector, this implementation must be accompanied by use of mutual exclusion

locks when the hub is run in a separate thread. In this case, the callbacks will invoke the add

data functions to write data into the MyoData queues in the worker thread stack frame. If

35

the data is then popped off of the queues in a the main thread using getFrameXXX()

without protecting these critical sections, the result is undefined.

3.1.3 MEX Interface

The Myo SDK implementation shown in the previous section provides the solution

to our implementation needs in the C++ environment. However, an additional layer is

required to enable calling of this C++ code from the MATLAB environment. The MATLAB

MEX interface provides C/C++ and Fortran bindings to the MEX API as well as MEX

compilation tools. These resources allow developers to write and compile so-called MEX

functions which are binary executable files that can be called from the MATLAB runtime in

m-code. In this section we will devise a strategy to execute the Myo SDK implementation in

such a way that it can be used transparently from m-code.

In general, MEX functions are simply external code that has been written and

compiled against the MATLAB MEX API. Since our external code language is C++, we will opt

to use the C-API. All such MEX files begin with the inclusion of the MEX header file and the

default entry point for a MEX function, void mexFunction(...). In this case, we choose

to name the MEX function source code file myo_mex.cpp so that MATLAB built-in function

mex will compile a binary file myo_mex.mexw64 (on 64 bit Windows platforms) that can

be called in MATLAB m-code as [...] = myo_mex(...).

The minimal example of myo_mex.cpp is shown below. In addition to the MEX

header, we also include the necessary files to compile against the Myo SDK and our inline

application classes in myo_mex.cpp. We also realize that since we can lock the memory for

36

the scope of myo_mex.cpp we declare collector, pHub, and pMyo in global scope so they

will be persistent variables.

#include <mex.h> // mex api

#include "myo/myo.hpp" // myo sdk library

#include "myo_class.hpp" // myo sdk implementation application classes

...

DataCollector collector

myo::Hub* pHub

myo::Myo* pMyo

...

void mexFunction(int nlhs, mxArray *plhs[],

int nrhs, const mxArray *prhs[])

{

...

}

At this point, myo_mex.cpp will compile, but its functionality down not yet exist.

Here we contemplate the major function of the MEX API which is to enable the passing of

variables from m-code as input parameters and back to m-code as return parameters or

outputs. The MEX API connects C++ variables back to a MATLAB workspace through the

arrays of pointers to mxArray. Note that mxArray *plhs[] and *prhs[] are pointers

for left-hand-side and right-hand-side parameters, respectively. Using these faculties, we can

build a state machine that consists of two states, three possible state transitions, and

initialization and delete transitions into and out of the MEX function. The transition requests

can be passed as strings in the first input parameter, and then the mexFunction() will

attempt to perform the requested transition while servicing any additional inputs and

outputs that exist.

The prototype state machine is shown below in Figure 3-1. The first call into MEX

must be on the init transition to initialize the Myo SDK implementation and lock the MEX

function memory region in the MATLAB process using the mexLock() MEX API. Then, the

default idle state is entered. Successful entry into the idle state signifies that the MEX function

37

is initialized without unrecoverable error and thus is ready to be used for streaming data.

The only permissible transition out of the idle state that is useful is start_streaming

which launches threaded calling of myo::Hub::runOnce() to begin streaming data and

places the MEX function into the streaming state. While in the streaming state, calls to the

get_streaming_data transition will acquire a lock on the data streaming thread, read all

available data from the data queues, and then return these data samples to the MATLAB

workspace while the MEX function returns to the streaming state. When in the streaming

state, data streaming can be cancelled by sending the MEX function back to the idle state with

the stop_streaming transition. Finally, at the end of the usage cycle, the delete

transition is used to clean up all Myo SDK resources and return the MEX function memory

management back to the control of MATLAB by calling the mexUnlock() MEX API.

Figure 3-1: MEX function states, transitions, and actions

A summary of this state machine implementation contained within

myo_mex.cpp is shown below with pseudocode descriptions of the intended actions

documented in the comments. Note that most of the business logic and MEX API code has

been omitted here for clarity.

38

void mexFunction(int nlhs, mxArray *plhs[],

int nrhs, const mxArray *prhs[])

{

// check input

char* cmd = mxArrayToString(prhs[0]); // get command string

if ( !strcmp("init",cmd) ) {

// initialize pHub and collector

mexLock(); // lock the memory region for this MEX function

} else if ( !strcmp("start_streaming",cmd) )


// dispatch thread calling pHub->runOnce()

} else if ( !strcmp("get_streaming_data",cmd) ) {

// create MATLAB struct outputs in *plhs[]

// read data queues using collector.getFrameXXX(id)

// assign data to outputs in *plhs[]

} else if ( !strcmp("stop_streaming",cmd) ) {

// terminate thread and reset state

collector.addDataEnabled = false;


} else if ( !strcmp("delete",cmd) ) {

// clean up Myo SDK and platform resources

mexUnlock();

}

return;

}

Although we will not fully describe the implementation of the MEX function in

detail, we will show specific example implementation for each of the MEX interface calls in

the following. The complete source code for myo_mex.cpp can be found in Appendix A.2 as

well as the public GitHub repository for this project [16].

Before dispatching any of the myo_mex commands, the MEX function must use

the MEX APIs to extract the command string from the input parameters in *prhs[]. The

first lines of mexFunction() perform these operations in addition to input error checking.

// check for proper number of arguments

if( nrhs<1 )

mexErrMsgTxt("myo_mex requires at least one input.");

if ( !mxIsChar(prhs[0]) )

mexErrMsgTxt("myo_mex requires a char command as the first input.");

if(nlhs>1)

mexErrMsgTxt("myo_mex cannot provide the specified number of outputs.");

char* cmd = mxArrayToString(prhs[0]);

39

Here we see the use of the integer nrhs to validate the minimum expected

number of inputs as well as additional MEX APIs to deal with the string data type expected

to be passed as the first input argument. At any time throughout this validation process, the

default behavior for an error condition is to throw an exception back to the MATLAB

workspace by use of the MEX API mxErrMsgTxt().

Once we have the character array cmd, a collection of if ... else if blocks

are used to take action matching each possible cmd. The bodies of the init and delete

commands are not covered here since they merely instantiate and destroy all Myo SDK

resources as described previously while locking memory space by bracketing these

operations with the MEX APIs mexLock() and mexUnlock(). The init method

additionally parses one required input argument that specifies countMyosRequired for

the session.

The start_streaming API is responsible for dispatching a worker thread to

call myo::Hub::runOnce() on pHub. In this work, we implement the thread using the

Windows API since we’re developing strictly for Windows, additional external libraries will

add unnecessary complexity to installation for the target end users, and a newer compiler

that offers threading support in the C++ standard library will be restrictive to new users

working on machines with older software.

if ( !strcmp("start_streaming",cmd) ) {

if ( !mexIsLocked() )

mexErrMsgTxt("myo_mex is not initialized.\n");

if ( runThreadFlag )

mexErrMsgTxt("myo_mex is already streaming.\n");

if ( nlhs>0 )

mexErrMsgTxt("myo_mex too many outputs specified.\n");


// dispatch concurrent task

runThreadFlag = true;

40

hThread = (HANDLE)_beginthreadex( NULL, 0, &runThreadFunc,

NULL, 0, &threadID );

if ( !hThread )

mexErrMsgTxt("Failed to create streaming thread!\n");

}

This command action toggles collector to begin responding to add data calls,

sets the runThreadFlag global variable, and then dispatches the thread to call

runThreadFunc(). This thread function will perform its routine until the

runThreadFlag is unset later in a call to stop_streaming.

unsigned __stdcall runThreadFunc( void* pArguments ) {

while ( runThreadFlag ) { // unset runThreadFlag to terminate thread

// acquire lock then write data into queue

DWORD dwWaitResult;

dwWaitResult = WaitForSingleObject(hMutex,INFINITE);

switch (dwWaitResult)

{

case WAIT_OBJECT_0: // The thread got ownership of the mutex

// --- CRITICAL SECTION - holding lock

pHub->runOnce(STREAMING_TIMEOUT); // run callbacks to collector

// END CRITICAL SECTION - release lock

if (! ReleaseMutex(hMutex)) { return FALSE; } // bad mutex

break;

case WAIT_ABANDONED:

return FALSE; // acquired bad mutex

}

} // end thread and return

_endthreadex(0); //

return 0;

}

The get_streaming_data command is then available only when

runThreadFlag is set indicating existence in the streaming state. This command action

begins with input and output checking before initialization of the variables used in the data

reading routine described previously. The new elements in this command action include

acquiring the mutual exclusion lock while reading data and handling the output data by

declaring MEX API types with mxArray *outDataN, initializing them with

makeOutputXXX(), assigning each frame using fillOutputXXX(), and finally assigning

41

the data arrays to output variables using mxCreateStructMatrix() and

assnOutputStruct().

if ( !strcmp("get_streaming_data",cmd) ) {



if ( !runThreadFlag )

mexErrMsgTxt("myo_mex is not streaming.\n");

if ( nlhs>1 )


// Verify that collector still has all of its Myos


if ( countMyos != countMyosRequired )

mexErrMsgTxt("myo_mex countMyos is inconsistent… We lost a Myo!");

// Declare and initialize to default values the following:

// iiIMU1 iiIMU2 iiEMG1 iiEMG2 szIMU1 szIMU2 szEMG1 szEMG2

// frameIMU1 frameIMU2 frameEMG1 frameEMG2

// Output matrices hold numeric data

mxArray *outData1[NUM_FIELDS];


// Initialize output matrices

makeOutputIMU(outData1,szIMU1);

makeOutputEMG(outData1,szEMG1);



// Now get ahold of the lock and iteratively drain the queue while

// filling outDataN matrices

DWORD dwWaitResult;



{



// Use handle the data frame for sensor N by using:

// fillOutputIMU(outDataN,frameIMUN,iiIMUN,szIMUN);

// fillOutputEMG(outDataN,frameEMGN,iiEMGN,szEMGN);


if ( !ReleaseMutex(hMutex))

mexErrMsgTxt("Failed to release lock\n");

break;


mexErrMsgTxt("Acquired abandoned lock\n");

break;

}

// Assign outDataN matrices to MATLAB struct matrix

plhs[DATA_STRUCT_OUT_NUM] = mxCreateStructMatrix(1,countMyos,

NUM_FIELDS,output_fields);

assnOutputStruct(plhs[DATA_STRUCT_OUT_NUM], outData1, 1);

42

if (countMyos>1) {


}

}

The way that we approach assigning the data frames to output variables in

*plhs[] when reading the values iteratively is to first initialize numerical arrays for each

individual data matrix that will be returned. There is one matrix for the data stored in each

of the fields of the FrameXXX objects. These matrices are objects of type mxArray named

outDataN. During the iterative reading of data from collector, we fill the corresponding

elements of outDataN with the data from the current frame. Then when all of the data has

been read from collector into the matrices in outDataN, we assign each of the matrices

to fields of MATLAB structure array in *plhs[] so that these fields correspond with the

fields of both FrameIMU and FrameEMG.

The MEX file can then be compiled by using the built in MATLAB command, mex.

This command must be passed the locations of the Myo SDK include and lib directories,

then name of the runtime library for the local machine’s architecture (32 or 64 bit), and the

location of myo_mex.cpp. Assuming that Myo SDK has been extracted to the path “C:\sdk\”

on a 64 bit machine, the following command will be used to compile myo_mex.cpp.

mex -I"c:\sdk\include" -L"c:\sdk\lib" -lmyo64 myo_mex.cpp

In this project a general build tool build_myo_mex() has been written for

which a user can issue the following command in the command window to perform this same

operation.

build_myo_mex c:\sdk\

43

Successful compilation of the MEX file results in the existence of a new file,

myo_mex.mexw64 (on a 64 bit machine) which is the executable that can be called from m-

code. All that remains now is for the user to call an accepted sequence of commands on

myo_mex while Myo Connect is running and connected to the desired number of Myo

devices. This is performed in m-code in an example that gathers five seconds of data from

the device as shown here.

countMyos = 1;

myo_mex(‘init’,countMyos);

myo_mex(‘start_streaming’);

pause(5);

d = myo_mex(‘get_streaming_data’);

myo_mex(‘stop_streaming’);

myo_mex(‘delete’);

% Data is now accessible in the matrices stored in fields of d:

% d.quat, d.gyro, d.accel, d.pose, d.arm, d.xDir, d.emg

This approach works well for single shot data log collection, but breaks down

when successive calls of get_streaming_data are required. This is the case when a

continuous stream of data is desired in the MATLAB workspace. The solution to this problem

is to encapsulate the calling pattern on myo_mex into a MATLAB class MyoMex that will

automatically implement the above data collecting routine to populate class properties with

the continuously streaming data from the Myo device(s).

3.1.4 Myo MATLAB Class Wrapper

The final MATLAB m-code layer of this project includes the creation of two

MATLAB classes, MyoMex and MyoData. MyoMex will be responsible for all management of

the myo_mex environment, and it will own a vector of MyoData objects to which all new

data will be passed. The MyoData object is a representation of the data from a Myo device.

44

When it receives new data from MyoMex, this data is pushed onto data logs. The MyoData

class also offers convenient ways to access and interpret the logged data.

By design, the usage of MyoMex has been made as simple as possible. The minimal

use case for this code is shown here.

mm = MyoMex; % MyoMex(countMyos) defaults to a single Myo

md = mm.myoData; % Use MyoData object to access current data logs

% md.quat, md.gyro, md.accel, md.emg, etc.

mm.delete(); % Clean up when done

All setup is performed in the constructor MyoMex(), and all cleanup is performed

in the overridden delete() method. Between these calls, when MyoMex is running, it is

continually calling myo_mex(‘get_streaming_data’) and passing this data to its

MyoData objects. The MyoData object can then be used to access the data. The process

diagram for MyoMex lifecycle including operations that cross the MEX boundary is shown in

Figure 3-2.

45

Figure 3-2: Myo MATLAB class wrapper behavior

The myo_mex command invocation has also been wrapped in static methods of

the MyoMex class. Each command has its own implementation with the signature

[fail,emsg,plhs] = MyoMex.myo_mex_<command>(prhs) in which a try ...

catch block catches myo_mex errors and returns the failure status along with any existing

error messages. In this way MyoMex can ensure the proper functionality of myo_mex by

attempting recovery if an unexpected failure is encountered.

The constructor for MyoMex calls into myo_mex(‘init’,countMyos) using

the static method wrapper, instantiates its myoData property, and then calls the

startStreaming() method to set up automatic polling for data.

46

function this = MyoMex(countMyos)

% validate preconditions for MyoMex and myo_mex

[fail,emsg] = this.myo_mex_init(countMyos);

% if fail, attempt recovery, otherwise throw error; end

this.myoData = MyoData(countMyos);

this.startStreaming();

end

The constructor for MyoData instantiates a vector of MyoData objects of length

countMyos. The startStreaming() method of MyoMex creates and starts the update

timer that schedules MyoMex calls into myo_mex(‘get_streaming_data’).

function startStreaming(this)

% validate preconditions

this.timerStreamingData = timer(...

'busymode','drop',...

'executionmode','fixedrate',...

'name','MyoMex-timerStreamingData',...

'period',this.DEFAULT_STREAMING_FRAME_TIME,...

'startdelay',this.DEFAULT_STREAMING_FRAME_TIME,...

'timerfcn',@(src,evt)this.timerStreamingDataCallback(src,evt));

[fail,emsg] = this.myo_mex_start_streaming();

% if fail, issue warning and return; end

start(this.timerStreamingData);

end

Then the timerStreamingDataCallback() method completes the normal

behavior of MyoMex. We invoke the get_streaming_data command and then pass the

data into MyoData by passing it to the addData method of MyoData along with the

currTime property which holds the time in seconds since MyoMex was instantiated.

function timerStreamingDataCallback(this,~,~)

[fail,emsg,data] = this.myo_mex_get_streaming_data();

% if fail, clean up and throw error; end

this.myoData.addData(data,this.currTime);

end

The MyoData class exposes two main interfaces, each with important bits of

business logic behind them. The addData method is exposed only to the friend class

MyoMex, and is the entry point for new streaming data to MyoData. The new data is

47

processed internally before it is then pushed onto the data log properties of MyoData for

consumption in the way of public access from the MATLAB workspace by users.

The addData method receives the output data struct array returned by

myo_mex. This method then calls two more utility methods, addDataIMU() and

addDataEMG(), to push the new data onto the data log properties of MyoData. The very

first time the data logs are updated, a time vector is initialized for each of the IMU and EMG

data sources, and subsequent data values are interpreted as having been sampled at time

instants determined by the number of data samples and the sampling time for the data

source.

The data properties of MyoData all have two forms. The property given the base

name for the data source contains the most recent sample, and another version of this

property with “_log” appended will contain the complete time history log for the data

source. For example, the properties quat and quat_log contain the most recent 1 × 4

vector of quaternion data and the complete 𝐾 × 4 matrix of 𝐾 quaternion samples,

respectively. In the remainder of this discussion of data properties, we assume the

appropriate property, e.g. quat versus quat_log, based on the context while only referring

to the property by its short name.

There are seven data sources that are built into Myo: quat, gyro, accel, emg,

pose, arm, xDir. However, these representations of the data may not be the most

convenient for all applications. Since the nature of the MyoData object is such that its

intended use case will be on dynamically changing streaming data, we will also perform

online data conversion. This means that we will translate the data to other representations

in order to provide users with the most convenient data representation without the added

48

cost of computation or code complexity. A summary of these properties is given in Table 3-1

including information about which data sources are derived from others and a description

of the interpretation of the data source. Note that these data sources are expanded along the

first dimension to create their associated data log properties except for rot_log which is

expanded along the third dimension.

Table 3-1: MyoData data properties

Data sources in MyoData including descriptions of the data and which source(s) it is derived from if not a default data source.

Data Source

Derived From

Description

quat --- 1 × 4 unit quaternion, transforms inertial vectors to sensor coordinates

rot quat 3 × 3 rotation matrix corresponding with quat

gyro --- 1 × 3 angular velocity vector with components in sensor coordinates

accel --- 1 × 3 measured acceleration vector with components in sensor coordinates

gyro_fixed gyro

quat Rotated representation of gyro by quat to coordinates of the inertial fixed frame

accel_fixed accel quat

Rotated representation of accel by quat to coordinates of the inertial fixed frame

pose --- Scalar enumeration indicating current pose

pose_<spec> pose Scalar logical indication of pose given by spec: rest,

fist, wave_in, wave_out, fingers_spread, double_tap, unknown

arm --- Scalar enumeration indicating which arm Myo is being worn on

arm_<spec> arm Scalar logical indication of which arm Myo is being worn on given by spec: right, left, unknown

49

Data Source

Derived From

Description

xDir --- Scalar enumeration indicating which direction the Myo is pointing on the subject’s arm

xDir_<spec> xDir Scalar logical indication of x-direction given by spec: wrist, elbow, unknown

emg --- 1 × 8 vector of normalized EMG intensities in [−1,1]

Finally, the time vectors upon which the data is sampled are given by the

properties timeIMU_log and timeEMG_log. The EMG data is sampled on timeEMG_log

at 200Hz whereas all other data sources should are sampled at 50Hz on the timeIMU_log

vector.

3.2 Sphero API MATLAB SDK Development

In this section, we build on the background information about the available

Sphero APIs covered in section 2.2.2 by implementing a MATLAB interface with the selected

API option. In this work, we have chosen to create the device interface entirely in native

MATLAB m-code by leveraging the Instrument Control Toolbox Bluetooth object to

communicate with Sphero by way of its low-level serial binary Bluetooth API. In the

remainder of this section, we step through the development process beginning with

introducing the basic concepts of the API, and then the design and development of the three

layer class hierarchy that comprises Sphero API MATLAB SDK [15], [18].

3.2.1 Sphero API Concepts

The Sphero API is a serial binary protocol documented publicly by the Sphero on

their GitHub repository with the original company name, Orbotix [19]. The serial protocol

50

transmits all device control information and data through a single communication channel

as a stream of bits, or zeros and ones. The way that commands and data are interpreted from

this continuous stream of bits is by imposing upon it some sort of expected structure. The

description of these structured bits is the binary protocol that we will describe here through

some specific examples of commands and data transmissions. Once the protocol description

is complete, all that remains is to implement the protocol with properly formed procedures

to communicate with Sphero as we will encounter in the following sections.

The Sphero API defines three subdivisions of the bitstream referred to as packets.

The command packet (CMD) is used to transmit a command from the client to Sphero. The

response packet (RSP) is used to transmit a response for a CMD from Sphero back to the

client. And the message packet (MSG) contains asynchronous messages from Sphero to the

client.

These packets each have a specific structure in their representation at the byte

level. In the remainder of this section, we’ll describe collections of bytes in two ways. The

integral data types represented by a collection of bytes will be noted by [u]int<N> where N

is the number of bits and the presence of “u” indicates that it’s an unsigned integer. For

example, uint8 is an unsigned 8 bit integer, and int16 is a signed 16 bit integer. Also, the

unsigned value of a collection of bit or bytes will be given in hexadecimal notation using the

characters 0-9 and A-F followed by “h” or binary notation using the characters 0-1 followed

by a “b.” For example, the number seventy-four is 4Ah, FFh is two-hundred fifty-five, ten is

1010b, and 0111b is seven.

51

A Sphero API CMD is constructed by assembling the fields listed in Table 3-2. A

CMD is assembled by writing each field and then concatenating them in order from SOP1 to

CHK.

Table 3-2: Sphero API CMD fields A CMD is the concatenation of these fields: [SOP1|SOP2|DID|CID|SEQ|DLEN|<DATA>|CHK]

Field Name Description Value

SOP1 Start of packet 1 Constant value FFh

SOP2 Start of Packet 2 Bits 0 and 1 constain per-CMD configuration flags

111111XXb

DID Device Identifier Specifies which "virtual device" the command belongs to

---

CID Command Identifier Specifies the command to perform ---

SEQ Sequence Number Used to identify RSP packets with a particular CMD packet

00h-FFh

DLEN Data Length Computed from combined length of <DATA> and CHK

computed

<DATA> Data Payload Array of byte packed data (optional) ---

CHK Checksum Checksum computed

Each command that is defined by the Sphero API has a documented DID and CID,

which together uniquely identify the command. Each command also has its own definition of

the <DATA> array with corresponding DLEN. All commands support two configuration

options that are selected by settings bits 0 and 1 of SOP2. Bit 1 instructs Sphero to reset its

command timeout counter. Bit 0 instructs Sphero to provide a RSP to the CMD. If bit 0 of

SOP2 is unset, then the host application will receive no RSP, and therefore no indication of

Sphero’s success in interpreting the CMD. And the checksum is computed from all previous

52

bytes except SOP1 and SOP2 to guard against Sphero taking action on corrupted or

malformed commands.

A RSP is generated from Sphero to the client in response to a CMD if bit 0 of SOP2

was set in the generating CMD. The fields of a RSP are listed in Table 3-3.

Table 3-3: Sphero API RSP fields A RSP is the concatenation of these fields: [SOP1|SOP2|MRSP|SEQ|DLEN|<DATA>|CHK]



SOP2 Start of Packet 2 Specifies RSP packet type FFh

MRSP Message Response Indicates failure status of CMD interpretation

00h for success

SEQ Sequence Number Used to identify RSP packets with a particular CMD packet

echoed from CMD

DLEN Data Length Computed from combined length of <DATA> and CHK

computed

<DATA> Data Payload Array of byte packed data (optional) ---


When a RSP is received by the client, the beginning of the packet is identified by

the bytestream FFFFh. Then, the remaining bytes are consumed according to DLEN and the

CHK is recomputed for comparison. Assuming no failures in this procedure, a valid response

is received to indicate the success of a previous command with a matching SEQ and

optionally provide data to the client application.

A MSG is sent to the client from Sphero asynchronously. Since the occurrence of

these messages depends on the state of Sphero, and some of this state can be changed with

persistence, the client application must be prepared to receive MSG given by the fields listed

53

in Table 3-4. It is important to note that unlike the DLEN field for CMD and RSP, the DLEN

field for MSG is 16 bits. This uint16 value is sent over the wire most significant byte first.

Table 3-4: Sphero API MSG fields A MSG is the concatenation of these fields: [SOP1|SOP2|ID_CODE|DLEN|<DATA>|CHK]



SOP2 Start of Packet 2 Specifies MSG packet type FEh

ID_CODE Identifier Code Indicates the type of message ---

DLEN Data Length Computed from combined length of <DATA> and CHK, 16 bit

computed

<DATA> Data Payload Array of byte packed data ---


The asynchronous message packet is sent from Sphero to the client at any time.

These packets can be identified when read by the client by checking the value of SOP2, and

they contain structured data in <DATA> that is decoded based upon the type of message

being sent as specified by the message identifier code, ID_CODE. Various CMD packets

configure Sphero to generate asynchronous messages periodically based upon the

occurrence of events or the passing of some time duration. Because of the asynchronous

nature of MSG packets, the client must always be in a state that attempts to read and parse

either RSP or MSG packets and behave accordingly to store the response data locally and

optionally take action automatically when a MSG is received without interfering with the

synchronicity of the CMD-RSP packet flow.

Perhaps the best way to describe the process of encoding and decoding packets is

to show by example with a few select CMD and the associated RSP. The Ping() command

54

is the simplest type of CMD in that it contains no <DATA> and its purpose is to verify

connectivity with Sphero. Table 3-5

Table 3-5: Command definition for the Ping() command

* The value FDh is also acceptable for SOP2 ** This is an arbitrary SEQ for purposes of computing CHK

SOP1 SOP2 DID CID SEQ DLEN <DATA> CHK

FFh FFh* 00h 01h 37h** 01h --- C6h

The procedure for computing the checksum above is listed here in sequence.

1. Compute the sum of bytes DID through <DATA>

00h + 01h + 37h + 01h = 39h = 57

2. Compute the module 256 of this result

57 % 256 = 57 = 00111001b

3. Compute the bitwise compliment of this result

~00111001b = 11000110b = C6h

The expected RSP for this Ping() command is shown in Table 3-6. The SEQ has

been echoed to inform the client application of its generating CMD and the MRSP indicates

success.

Table 3-6: Response definition for the Ping() command

*This value for SEQ corresponds with that sent in the CMD constructed previosuly

SOP1 SOP2 MRSP SEQ DLEN <DATA> CHK

FFh FFh 00h 37h* 01h --- C7h

These examples so far have shown one CMD and one RSP each with no <DATA>

payload. Two more commands that are a bit more useful than Ping() are Roll() and

55

ReadLocator(). These commands are used to command Sphero to move with a desired

speed and heading, and to read Sphero’s current estimated position, respectively.

The Roll() command requires a data payload that contains an 8 bit SPEED, a 16

bit HEADING, and an 8 bit STATE. The value for SPEED is a normalized fraction of maximum

speed so that FFh is full speed and 00h is no speed. We’ll construct this command for roughly

half speed, with a SPEED of 80h. The HEADING is the planar heading specified in degrees

from 0 to 360. In this example we’ll take the heading to be 270° or 010Eh. As with all

multibyte values, the HEADING will be sent over the wire most significant byte first as the

pair 01h, 0Eh. The STATE is an enumeration selecting the mode with which the Roll()

command should be carried out. The normal mode is selected by specifying STATE 01h. The

following Table 3-7 summarizes this Roll() CMD.

Table 3-7: Command definition for the Roll() command

SOP1 SOP2 DID CID SEQ DLEN SPEED HEADING STATE CHK

FFh FFh 02h 30h 37h 05h 80h 01h 0Eh 01h 01h

Since we have specified that a RSP should be generated for this Roll() CMD, we

will expect a response that is similar in form to that received from the Ping() command

previously in Table 3-6.

The ReadLocator() CMD is specified by DID 02h and CID 15h, and it should

be constructed in a way that is similar to the Ping() CMD previously in Table 3-5 with SOP2

set so that a RSP will be generated. There is no data associated with the ReadLocator()

CMD, as its purpose is to query Sphero for <DATA> that is returned in the ReadLocator()

RSP. The RSP <DATA> for contains five 16 bit integers that encode Sphero’s current position,

56

velocity, and speed. The following Table 3-8 shows an example RSP assuming a CMD success

with a SEQ of 37h.

Table 3-8: Response <DATA> definition for the ReadLocator() command

DLEN X_POS Y_POS X_VEL Y_VEL SOG CHK

0Bh 00h 09h FFh C5h FFh 8Fh FFh 67h 00h BEh 3Eh

The interpretation of this RSP <DATA> is in units of centimeters and centimeters

per second as int16 datatype. The translation of this ReadLocator() <DATA> into values

with engineering units is captured in Table 3-9.

Table 3-9: Response <DATA> interpretation for the ReadLocator() command

Data Raw Value Engineering Value Description

X_POS 0009h 9cm 𝑥 component of planar position

Y_POS FFC5h −59cm 𝑦 component of planar position

X_VEL FF8Fh −113cm/s 𝑥 component of planar velocity

Y_VEL FF67h −153cm/s 𝑦 component of planar velocity

SOG 00BEh 190cm/s Speed over ground or magnitude of planar velocity

The third packet type is most useful in this project by its use in receiving

streaming data from Sphero. The SetDataStreaming() CMD sends configuration data to

Sphero to set up the streaming data messages. This configuration data includes information

about the desired sample rate, number of samples per data frame transmission, and total

number of packets to send in addition to specification of the desired data sources. The data

sources are selected by sending a 32 bit MASK that’s computed by performing a bitwise

logical OR operation on the mask bits for the desired data types shown in Table 3-10.

57

Table 3-10: Data source MASK bits for SetDataStreaming() command and message

Data Source Data Source MASK Bit Description

MASK_ACCEL_X_RAW 80h 00h 00h 00h Raw measured acceleration

MASK_ACCEL_Y_RAW 40h 00h 00h 00h

MASK_ACCEL_Z_RAW 20h 00h 00h 00h

MASK_GYRO_X_RAW 10h 00h 00h 00h Raw angular velocity

MASK_GYRO_Y_RAW 08h 00h 00h 00h

MASK_GYRO_Z_RAW 04h 00h 00h 00h

MASK_MOTOR_RT_EMF_RAW 00h 40h 00h 00h Raw right motor EMF

MASK_MOTOR_LT_EMF_RAW 00h 20h 00h 00h Raw left motor EMF

MASK_MOTOR_LT_PWM_RAW 00h 10h 00h 00h Raw left motor PWM

MASK_MOTOR_RT_PWM_RAW 00h 08h 00h 00h Raw right motor PWM

MASK_IMU_PITCH_FILT 00h 04h 00h 00h Filtered pitch angle

MASK_IMU_ROLL_FILT 00h 02h 00h 00h Filtered roll angle

MASK_IMU_YAW_FILT 00h 01h 00h 00h Filtered yaw angle

MASK_ACCEL_X_FILT 00h 00h 80h 00h Filtered measured acceleration

MASK_ACCEL_Y_FILT 00h 00h 40h 00h

MASK_ACCEL_Z_FILT 00h 00h 20h 00h

MASK_GYRO_X_FILT 00h 00h 10h 00h Filtered angular velocity

MASK_GYRO_Y_FILT 00h 00h 08h 00h

MASK_GYRO_Z_FILT 00h 00h 04h 00h

MASK_MOTOR_RT_EMF_FILT 00h 00h 00h 40h Filtered right motor EMF

MASK_MOTOR_LT_EMF_FILT 00h 00h 00h 20h Filtered left motor EMF

58

If we desire to configure both the raw and the filtered measured acceleration data

sources, then the MASK would be determined by OR-ing these MASK bits together as shown

in Table 3-11.

Table 3-11: Data source MASK for streaming accelerometer data

Data Source Data Source MASK Bit

MASK_ACCEL_X_RAW 80h 00h 00h 00h

MASK_ACCEL_Y_RAW 40h 00h 00h 00h

MASK_ACCEL_Z_RAW 20h 00h 00h 00h

MASK_ACCEL_X_FILT 00h 00h 80h 00h

MASK_ACCEL_Y_FILT 00h 00h 40h 00h

MASK_ACCEL_Z_FILT 00h 00h 20h 00h

MASK 11100000b 00000000b 11100000 00000000b

E0h 00h E0h 00h

The other parameters for the SetDataStreaming() command are introduced

and described in Table 3-12 along with example values to configure streaming the data

sources specified in MASK at a rate of 1Hz with one sample per data frame one time.

Table 3-12: Command parameters for SetDataStreaming command

The DID and CID for this CMD are 02h and 11h, respectively

Data Parameter Description Value

DLEN Data length depends upon existence of MASK2 0Ah

N Sample rate divider on 400Hz base frequency 0190h

M Samples per frame 0001h

MASK Selects data sources E000E000h

59

Data Parameter Description Value

PCNT Packet count: number of data frames to be transmitted (zero for unlimited streaming)

01h

MASK2

(optional)

Selects additional data sources by extending MASK ---

The complete SetDataStreaming() CMD would then be the following byte

string with a SEQ of 37h and the data parameters given in Table 3-12.

FFh FFh 02h 11h 37h 0Ah 01h 90h 00h 01h E0h 00h E0h 00h 01h 58h

Assuming that the SetDataStreaming() command has been successfully

configured to stream one sample per frame, then we expect to receive a MSG with ID_CODE

03h and DLEN 000Dh since we have selected 6 data sources that are each two bytes wide. An

example MSG for this scenario may be given by the following.

Table 3-13: Message definition for the DataStreaming message

SOP1 SOP2 ID_CODE DLEN <DATA> CHK

FFh FEh 03h 00h 0Dh See Table 3-14 62h

As the MSG is received in the incoming bytestream by the client, it is first identified

by the substring FFFEh formed by SOP1 and SOP2. Then the ID_CODE is used to identify

the type of MSG, and the DLEN is used to read the remainder of the MSG. Finally, the CHK is

computed by the client to check for possible data corruption. Successful parsing of this

particular MSG should result in a <DATA> payload similar to the example shown here. The

serial bytestream of signed 16 bit integers is taken to represent the selected bits in MASK in

the same order in which they appear in MASK from most significant bit to least significant

bit. Table 3-14 contains an example <DATA> payload for this particular MSG.

60

Table 3-14: Message <DATA> definition for the DataStreaming message

ACCEL_

X_RAW

ACCEL_

Y_RAW

ACCEL_

Z_RAW

ACCEL_

X_FILT

ACCEL_

Y_FILT

ACCEL_

Z_FILT

00h 00h 00h 0Ah 00h FBh FFh DFh 00h 76h 10h 24h

The data in Table 3-14 is translated from hardware units to engineering units

according to scaling factors listed in Sphero API documentation [19] and shown in Table

3-15.

Table 3-15: Message <DATA> interpretation for the DataStreaming message

Data Raw Value Engineering Value Description

ACCEL_X_RAW 0000h 0g Measured acceleration in units of g on an 8 bit scale

ACCEL_Y_RAW 000Ah 0.04g

ACCEL_Z_RAW 00FBh 0.98g

ACCEL_X_FILT FFDFh −0.01g Measured acceleration in units of g on a 12 bit scale

ACCEL_Y_FILT 0076h 0.03g

ACCEL_Z_FILT 1024h 1.01g

The encoding of CMD packets and the decoding of RSP and MSG packets along with

knowledge of the state changes that influence the creation of these packets comprise the

necessary prerequisites for implementing a programmatic client object to manage this

communication.

3.2.2 Sphero API Implementation

The MATLAB interface for Sphero is enabled by the implementation of a MATLAB

class named SpheroCore that manages all state and communication with Sphero through

a MATLAB Instrument Control Toolbox Bluetooth object. The first step toward

implementing this solution is to understand how to communicate with Sphero via the

61

Bluetooth object. Through examples similar to those given in the previous section, we

incrementally build the functionality of SpheroCore in this section. The end result is a

MATLAB class with methods to implement each of the Sphero API function calls and

properties to contain information about Sphero’s state and data.

The constructor for SpheroCore requires a single input argument that is used

to uniquely identify Sphero, its remote_name. This is of the form “Sphero-XXX” where

“XXX” is a three character string of initials representing the colors that Sphero blinks when

awake and idle. For example, a device with the color sequence white-white-purple will bear

the default remote_name “Sphero-WPP”. The constructor returns a handle to the

SpheroCore object only when instantiation of its Bluetooth object, bt, completes

successfully by opening of the Bluetooth device for communication. Note that most of the

validation code as well as the initialization of event listeners and local device state

representation has been omitted for clarity here.

function s = SpheroCore(remote_name)

% instantiate bluetooth property

s.bt = Bluetooth(...

remote_name,s.BT_CHANNEL,...

'BytesAvailableFcnMode','byte',...

'BytesAvailableFcnCount',1,...

'BytesAvailableFcn',@s.BytesAvailableFcn,...

'InputBufferSize',8192);

% open bt and call Ping() to validate connectivity

fopen(s.bt); pause(0.5);

assert( ~s.Ping(),'SpheroCore.Ping() failed.');

end

We notice two things here, the way in which the Bluetooth object is configured

and the way in which API functions are called on the SpheroCore object. Since we must

respond to incoming MSG packets asynchronously, it’s imperative that incoming data is read

from s.bt automatically by way of the BytesAvailableFcn callback function property.

62

The behavior of this callback function, the BytesAvailableFcn() method, will be

covered later during discussion of parsing RSP and MSG packets.

The flowchart for implementation of the Sphero API command methods is shown

in Figure 3-3 for reference as a guide for the following description of the code used to

implement the Ping(), Roll(), and ReadLocator() methods. Each API method sends

its DID, CID, and <DATA> along with per-command configuration flags to a stack of utility

methods. The utility methods construct the CMD packet, write it to Sphero, and optionally

wait for a response and return data if it exists.

Figure 3-3: Sphero API send flowchart

63

The Sphero API functions all share the same signature template. The parameter

list begins with the class handle reference followed by a list of API <DATA> parameters

required for the API function, and is completed by a variable length argument list where the

per-command configuration flags can optionally be passed to the API method.

function fail = Ping(s,varargin)

[reset_timeout_flag,~] = s.ParseVargs(varargin{:});

did = s.DID_CORE; cid = s.CMD_PING; data = [];

fail = s.WriteClientCommandPacket(did,cid,data,...

reset_timeout_flag,true);

end

First we parse the variable length argument array varargin using

ParseVargs(). The varargin array is assumed to contain only flags to set the command

timeout reset (reset_timeout_flag) and the command answer behavior

(answer_flag), in that order. Since Ping() must request a response to be useful, the

second output of ParseVargs() is ignored. The next thing to happen is the assignment of

the DID and CID from constant properties of the class containing this information and the

construction of the <DATA> array. In the case of Ping() there is not <DATA> so we

construct the empty matrix instead. Then we are ready to issue the command by sending this

command to Sphero via WriteClientCommandPacket().

function [fail,resp] = WriteClientCommandPacket(s,did,cid,data,...

reset_timeout_flag,answer_flag)

fail = []; resp = [];

sop1 = s.SOP1;

sop2 = s.SOP2_MASK_BASE;

if reset_timeout_flag

sop2 = bitor(sop2,s.SOP2_MASK_RESET_TIMEOUT,'uint8');

end

seq = 0; % default seq

if answer_flag

sop2 = bitor(sop2,s.SOP2_MASK_ANSWER,'uint8');

seq = s.GetNewCmdSeq();

end

% figure out dlen from data

dlen = length(data)+1;

% compute checksum beginning with did

64

chk = bitcmp(mod(sum(uint8([did,cid,seq,dlen,data])),256),'uint8');

packet = uint8([sop1,sop2,did,cid,seq,dlen,data,chk]);

fwrite(s.bt,packet,'uint8');

if answer_flag, [fail,resp] = s.WaitForCommandResponse(); end

end

The WriteClientCommandPacket() method constructs the packet byte

string from the provided DID, CID, <DATA>, and configurations flags by computing SOP2,

DLEN, and CHK. Then the packet is written to the Bluetooth object. Optionally, if

answer_flag is set, WaitForCommandResponse() is called to spin with a timeout

waiting to read a RSP. This method returns a failure status along with any RSP <DATA> that

has been passed back to the client.

function [fail,resp] = WaitForCommandResponse(s)

fail = true; resp = [];

tic; t = 0;

while fail && (toc < s.WAIT_FOR_CMD_RSP_TIMEOUT)

if ~isempty(s.response_packet) && (s.response_packet.seq == s.seq)

% check for successful response

fail = s.CheckResponseFailure();

dlen = s.response_packet.dlen;

if ~fail && (dlen > 0)

resp = s.response_packet.data;

end

s.response_packet = [];

else

pause(s.WAIT_FOR_CMD_RSP_DELAY);

end

t = toc;

end

end

The method WaitForCommandResponse() spins in a while ... end loop

with a timeout waiting for the response_packet property to be filled with response data

by BytesAvailableFcn(). If a response is found, the failure and possibly response data,

are returned up the call stack to the originating Sphero API method when the data will be

interpreted and the failure status is returned to the base calling workspace.

65

The implementation of the Roll() API method shows the way in which API

<DATA> parameters may be passed through to be written in the CMD packet.

function fail = Roll(s,speed,heading,state,varargin)

% input validation


switch state

case 'normal'

roll_state = s.ROLL_STATE_NORMAL;

case 'fast'

roll_state = s.ROLL_STATE_FAST;

case 'stop'

roll_state = s.ROLL_STATE_STOP;

end

speed = round(255*speed);

heading = floor(wrapTo360(heading));

heading_arr = s.ByteArrayFromInteger(heading,'int16');

did = s.DID_SPHERO; cid = s.CMD_ROLL;

data = [speed,heading_arr,roll_state];

fail = s.WriteClientCommandPacket(did,cid,data,...

reset_timeout_flag,answer_flag);

end

And the implementation of the ReadLocator() API method shows the way in

which RSP <DATA> is returned to, and handled by, an API method.

function [fail,locator_data] = ReadLocator(s,varargin)


did = s.DID_SPHERO; cid = s.CMD_READ_LOCATOR; data = [];

[fail,data] = s.WriteClientCommandPacket(did,cid,data,...

reset_timeout_flag,true);

% handle response data

locator_data = [];

if fail || isempty(data) || (10~=length(data))

fail = true;

return;

end

s.time = s.time_since_init;

% process data

[x,y,dx,dy,v] = s.LocatorDataFromData(data);

s.odo = [x,y];

s.vel = [dx,dy];

s.sog = v;

locator_data.odo = [x,y];

locator_data.vel = [dx,dy];

locator_data.sog = v;

end

66

The majority of the code in this method is dealing with the handling of response

data. Since the data is also associated with SpheroCore properties, it is assigned to the

appropriate properties, odo, vel, and sog, as well as it is assigned to an output structure

locator_data in order to be returned to the caller.

All the time when a valid instance of SpheroCore exists, the

BytesAvailableFcn() method is receiving callbacks for the event that new incoming

data is available on the Bluetooth object s.bt. The process flow for the reading of

incoming data is shown in Figure 3-4. Since the reading of packet data can void the need for

the callback to execute a number of times, we keep track of the number of extra bytes read

within this callback function. If the num_bytes to skip is positive, we decrement this value

and return. Otherwise, the SpinProtocol() method is called .

Figure 3-4: Sphero API receive flowchart

Within the SpinProtocol() method the incoming data is read in sequence to

identify the start of packet for a RSP with FFFFh or a MSG with FFFEh. Once the incoming

67

packet type is identified, the remainder of the packet is read using DLEN, num_bytes is

accumulated, and the appropriate action is taken. For a RSP packet, the response_packet

property is set so that WaitForCommandResponse() can receive the response data. A

MSG will trigger the invocation of the appropriate MSG handler. The implementation of

SpinProtocol() is shown in Appendix A.3 for reference.

In addition to providing the means by which to call Sphero API functions on

Sphero, SpheroCore also keeps track of all of the device data received from Sphero. This is

performed by the maintenance of class properties for each data source. Table 3-16 shows a

list of all data sources.

Table 3-16: Sphero data sources

Data Source Description

accel_raw 1 × 3 Raw measured acceleration in sensor frame

gyro_raw 1 × 3 Raw angular velocity in sensor frame

motor_emf_raw 1 × 2 Raw motor EMF (left, right)

motor_pwm_raw 1 × 2 Raw motor PWM (left, right)

imu_rpy_filt 1 × 3 Filtered roll, pitch, yaw determined from quat

accel_filt 1 × 3 Filtered acceleration magnitude

gyro_filt 1 × 3 Filtered angular velocity

motor_emf_filt 1 × 2 Filtered motor EMF (left, right)

quat 1 × 4 Estimated unit quaternion, orientation of the worl with respect to the sensor frame

rot 3 × 3 Rotation matrix determined from quat

accel_one Scalar magnitude of measured acceleration

odo 1 × 2 Planar position of Sphero

68

Data Source Description

vel 1 × 2 Planar velocity of Sphero

sog Scalar speed over ground, magnitude of planar velocity

Each of the data sources shown in Table 3-16 exist as properties of SpheroCore

that can be accessed to receive the most recently known value for the data source. If no value

has been reported up to the current time, a NaN value is used as a placeholder. Consecutive

instances of values received for these data sources are also stored in log properties that are

given similar names, with “_log” appended. Two more properties, time and time_log

hold the time since SpheroCore initialization at which data values were received in the

MATLAB environment.

A minimal use case for SpheroCore may be considered as one in which we

connect to Sphero, use a for ... end loop to roll Sphero around in a circle, and then stop.

This example is implemented in the following code snippet.

s = SpheroCore(‘Sphero-WPP’);

speed = 0.5; state = ‘normal’;

reset_timeout_flag = []; answer_flag = false;

for heading = 1:10:360 % roll in a circle

s.Roll(speed,heading,state,…

reset_timeout_flag,answer_flag)

pause(0.5);

end

s.Roll(0,0,’normal’); %stop Sphero

s.delete();

clear s;

3.2.3 Sphero Interface

The implementation of SpheroCore follows as closely as possible the patterns

evident in the Sphero API specification. However, in some cases, this documented behavior

is less than ideal. It is for this reason that this project also provides a subclass of

SpheroCore named SpheroInterface as well as another nested subclass Sphero. The

69

former provides convenience methods to extend and override the default behavior of

SpheroCore, whereas the latter is intended to be a template application class for users to

implement for their usage of SpheroInterface.

The primary extension that SpheroInterface offers to SpheroCore is a

redefinition of the heading used in the Roll() method and the coordinate system used

with both Roll() and ConfigureLocator() (and thus ReadLocator()). First, an

overridden Roll() method negates the value passed in for heading so that

SpheroInterface works with a right-handed coordinate system instead of the default

left-handed rotation angle. Once this preferred convention is imposed, we resolve the

remaining inconvenience with Sphero’s coordinate system. Rotating the reference

coordinate system used in Roll() and ReadLocator() with SpheroCore would require

external management of a consistent rotation angle to be added to the heading for Roll()

and used to configure the locator coordinates before calling ReadLocator(). We simplify

this process and improve the user experience when working with rotation calibrated

coordinates in SpheroInterface. A new property heading_offset keeps track of the

desired rotation. A new method, ConfigureLocatorWithOffset() is used to call

ConfigureLocator() with the heading_offset used to calculate a consistent

yaw_tare argument. And the RollWithOffset() method is a wrapper for Roll() that

adds the heading_offset rotation angle to the heading argument.

Additional modifications such as these may be incorporated by users in the

Sphero class that inherits from SpheroInterface. Some type of application data, or

perhaps a generic userdata property, may be useful to have contained within the Sphero

70

instance in a user’s program. This framework supports the implementation of such an

application specific class.

3.3 Application Cases

There are many possibilities for use with each of these software projects

individually, and even more when they are combined. In this section, we take a cross section

of the possible use cases to showcase some of the ways in which these projects can be used.

For each of Myo and Sphero we’ll look at both examples that use the command line interface

(CLI) as well as prototypical graphical user interface (GUI) applications. Then, finally, we’ll

look at an application that combines the sensor data from both Myo and Sphero that serves

as the conceptual basis for the human motion analysis application that is the focus of the

other remainder of this work.

Videos that show the basic CLI and GUI capabilities packaged with these software

tools for Myo and Sphero can be found at the following locations on YouTube:

Myo: https://www.youtube.com/watch?v=pPh306IgEDo

Sphero: https://www.youtube.com/watch?v=YohxMa_z4Ww

3.3.1 Myo Command Line Interface

The Myo is primarily an input device that may be used to collect raw sensory data

from any of its IMU sources or its eight channel surface EMG sensors. A typical data collection

application that is well suited for implementation in a MATLAB m-code script is the task of

data collection for an experimental trial. In the following example, we create an instance of

MyoMex, wait DURATION seconds, and then access the desired data before cleaning up the

connection.

https://www.youtube.com/watch?v=pPh306IgEDo

https://www.youtube.com/watch?v=YohxMa_z4Ww

71

DURATION = 5; % seconds

mm = MyoMex;

pause(DURATION);

emg = mm.myoData.emg_log;

time = mm.myoData.timeEMG_log;

mm.delete();

figure;

plot(time,emg);

ylabel('emg [normalized activation]');

xlabel('time [s]');

title(sprintf('EMG versus time for a %d[s] trial',DURATION));

xlim([0,DURATION]);

Once we have collected the emg data and its corresponding time vectors, we can

operate on these MATLAB workspace variables in a way of our choosing. Although it’s not

shown here, one may choose to save() the variables in a “.mat” file for future analysis.

Instead, we plot the data to inspect it visually as shown in Figure 3-5.

Figure 3-5: Myo CLI EMG logger plot

72

Figure 3-5 shows example EMG data from Myo in the format with which it is

stored in MyoData. This is eight dimensional data representing the EMG activation on a

normalized scale in the range [−1,1] that is sampled at a rate of 200Hz. The 1000 samples of

eight dimensional data in this plot are show without a legend or other signal disambiguation

since the data is shown exclusively for illustrative purposes.

3.3.2 Myo Graphical User Interface

The Myo SDK MATLAB MEX Wrapper project comes packaged with a collection of

two GUIs. These GUIs are used to instantiate and destroy the MyoMex object and to monitor

the data from each Myo using its corresponding MyoData instance.

The MyoMexGUI_Monitor is used to control the creation and deletion of

MyoMex objects. This GUI, shown in Figure 3-6, contains prompts for users to specify the

desired number of Myos to be used in a new session before instantiating MyoMex by clicking

the “Init MyoMex” pushbutton. Following instantiation, the same pushbutton changes to

provide functionality to “Delete MyoMex.” Once MyoMex is created, users can launch either

one or two instances of the MyoDataGUI_Monitor by selecting the appropriate

checkbox(es). The possible number of MyoDataGUI_Monitor instances depends on the

number of Myos that have been connected to in the current session.

73

Figure 3-6: Myo GUI MyoMexGUI_Monitor

The MyoDataGUI_Monitor, shown in Figure 3-7, is designed to receive a handle

reference to a MyoData object as its first input argument. The GUI manages the creation and

deletion of its own timer object to schedule graphical updates. The timer callback function

receives the MyoData handle reference as an additional parameter to access Myo data for

use in the visualizations provided as the main features of this GUI.

Figure 3-7: Myo GUI MyoDataGUI_Monitor

The right hand side of the GUI contains strip charts showing the most recent data

from the quat, gyro, accel, and emg sources along with a representation of the signal

magnitude for the latter three sources (thick line). The top left of the GUI contains

pushbutton controls to toggle the streaming status and radio buttons to toggle the

74

representation of the vector data shown in the strip charts. The gyro and accel data can

be viewed in coordinates of either the sensor or fixed frame of reference.

The left side of the GUI also shows a visualization of the quat data. The cylinder

in this plot represents the subject’s lower arm, with the elbow at the origin. Slider controls

are used to allow the user to reorient the fixed frame on the screen so that correspondence

between the virtual model (cylinder) and physical model (subject lower arm) can be

obtained by manual adjustment. Also on the left side, at bottom, an indication of detected

pose is displayed when this is not either pose_rest or pose_unknown.

3.3.3 Sphero Command Line Interface

Sphero is typically used in one of two types of applications. A teleoperative

application will utilize API functions such as Roll() and possibly ReadLocator() that

enable closed loop control of Sphero’s trajectory. Since an example using Roll() has

already been provided earlier at the end of section 3.2.2, we’ll show the alternative use of

Sphero as an input device here with a data logging example. In the following example, we

create an instance of Sphero, toggle off stabilization mode, set data streaming for

DURATION seconds, collect the data, and then reset stabilization before cleaning up the

connection to Sphero.

DURATION = 5; % seconds

RATE = 50; % Hz

sensors = {'gyro_raw','gyro_filt'};

s = Sphero('Sphero-WPP');

s.SetStabilization(false);

s.SetDataStreaming(RATE,RATE,DURATION,sensors);

pause(DURATION+1);

t = s.time_log;

gr = s.gyro_raw_log;

gf = s.gyro_filt_log;

s.SetStabilization(true);

s.delete();

75

figure;

plot(t-t(1),gr,'-'); hold on;

plot(t-t(1),gf,':');

legend({'raw','','','filt','',''});

ylabel('gyro [deg/s]');

xlabel('time [s]');

title(sprintf('%s for a %d[s] trial',...

‘Gyro (raw and filt) versus time ‘,DURATION));

xlim([0,DURATION]);

The configuration of streaming data in this example was to stream DURATION

seconds of both raw and filtered gyroscope data at a sample rate of RATE Hz with RATE

samples per data frame, or one data frame transmitted per second. The resulting 250

samples of the six signals are shown in the plot of Figure 3-8. Although one may otherwise

choose to save the data log for future analysis, here we’re only interested in visualizing the

data in a plot.

Figure 3-8: Sphero CLI gyroscope logger plot

76

Figure 3-8 shows the two variants of gyroscope data available from Sphero, both

raw and filtered. The raw data is shown in solid lines and the filtered data is shown in the

dotted lines. As is evident in the plot, when this data was collected, Sphero was subjected to

a rotation about one of its sensor frame’s basis vector axes. Then after a brief pause, it was

rotated about the same axis in the opposite direction. Correctness of the unit conversion can

also be roughly checked here as the 1.5s duration rotation at −1000°/s and the 2s rotation

at about 1250°/s represent a reasonable 3 to 3.5 revolutions per second.

3.3.4 Sphero Graphical User Interfacess

The Sphero API MATLAB SDK package is distributed with a collection of GUIs that

provide users with a way to quickly get started with programmatic interactions with Sphero

as well as a starting point for further development efforts. The multiple GUIs provide

features to control the Sphero object including setting the device color and configuring its

heading_offset among examples of two application cases for the device. A vector drive

application allows users to drive Sphero while visualizing its odometry data, and a

visualization application allows users to view its IMU data while manipulating the device.

Users may choose to first launch the SpheroGUI_MainControlPanel, shown

in Figure 3-9, to manage the connection to Sphero and launch the other packaged GUIs. In

the event that a user doesn’t know the remote_name for the Sphero device, the “Find

Devices” pushbutton performs a search on the local machine for paired Bluetooth device

candidates. The remote_name is either selected from the list of found devices or entered

manually before clicking the “Connect” pushbutton to begin a session with Sphero. Once

Sphero is connected, this GUI is then used to launch the other GUIs, and then finally clean up

the session by clicking the “Disconnect” pushbutton.

77

Figure 3-9: Sphero GUI SpheroGUI_MainControlPanel

Also shown in this figure are the GUIs: SpheroGUI_ChangeColor (upper right) and SpheroGUI_ConfigHeadingOffset (lower right)

Two utility GUIs are provided to change Sphero’s color and to configure the

heading_offset as shown at right in Figure 3-9. The color of Sphero is selected by

clicking a color wheel shown in SpheroGUI_ChangeColor, which automatically calls

SetRGBLEDOutput() to update the color displayed by Sphero. The heading_offset

property of SpheroInterface is configured by changing this value using the slider in

SpheroGUI_ConfigHeadingOffset. The slider callback function first updates the

heading_offset property before calling the RollWithOffset() and

ConfigureLocatorWithOffset() methods of SpheroInterface to update Sphero

with the new rotated locator coordinates. Sphero is commanded to roll with no speed to

orient the device with the rotated coordinate system. Since the back light on Sphero is

illuminated by SetBackLEDOutput() when this GUI is open, the user is given feedback on

the current orientation of the locator coordinates.

The first of two example applications showcased in Sphero GUIs is the vector drive

application shown in Figure 3-10. This GUI uses a timer object to schedule updates that

issue streaming (without answer_flag set) calls to RollWithOffset() and refresh the

78

display of Sphero’s odometry data. The vector drive input is provided by the user who will

click and hold the mouse in the white circle at left in Figure 3-10. The vector drawn from the

center of the circle to the mouse location, illustrated by the red dotted arrow, determines the

speed and heading for Sphero by its magnitude and angle, respectively. When this GUI is

open, Sphero is also configured to stream its odometry data odo and vel, its position and

velocity, to enable graphical updates with this information as shown at the right of this figure.

The trace of its position is displayed along with its current bearing given by a vel. The

bearing is in the plot shown as a short line segment originating from the center of Sphero.

Figure 3-10: Sphero GUI SpheroGUI_Drive

The second example application for Sphero is the input data visualization shown

in Figure 3-11 named SpheroGUI_VisualizeInputData. Similar to the CLI example,

the stabilization is turned off when this GUI is active. Sphero is set to stream its quaternion,

gyroscope, and accelerometer data at a default sample rate of 40Hz and frame rate of 10Hz.

A timer object is used to schedule graphics updates on the plot elements.

79

Figure 3-11: Sphero GUI SpheroGUI_VisualizeInputData

Strip charts of the most recent time series data from the gyroscope and

accelerometer are shown at right. Radio buttons at top right are used to change the

coordinate system in which this vector data is represented between “Robot” (sensor) and

“World” (fixed) frame. At left, a spatial visualization of the quaternion data displayed along

with a slider control that allows the user to orient the fixed coordinates with the screen

coordinates. This allows the user to register the orientation of the virtual model with that of

the physical model.

3.3.5 Myo and Sphero Motion Capture

The application that is the focus of this work has been chosen here to be the

application case that employs both Myo and Sphero simultaneously. The

MyoSphero_UpperLimb GUI shown in Figure 3-12 represents a very simple “zeroth-

order” approximation to the use of two Myo devices and a Sphero device as an upper limb

motion capture system. The subject wears one Myo on each of the upper and lower arm

segments and holds a Sphero with the hand. Then, the lengths of the subject’s upper limb

80

segments are assumed and the orientations of each Myo and Sphero are assumed to be

exactly the orientations of the corresponding anatomical segments. The forward kinematics

of this virtual model are updated on a schedule with a timer object to provide a near real

time virtual model of the subject’s physical upper limb.

Figure 3-12: Myo and Sphero upper limb motion capture

The controls at left of Figure 3-12 are first used to initialize the Myos and Sphero.

Then, two more configuration options exist for the upper limb virtual model. Since the

application doesn’t support an automatic way to detect which arm is being used, the “Switch

Arm” push button is used to reflect the graphical representation of the upper limb segments

in the plot. The Myo SDK does provide a data source that indicates the likely arm on which

the device is worn, but tests have shown this result to be less reliable than manual

configuration by the user. A “Toggle Myos” checkbox also allows users to change the order

in which the two Myos are taken to represent the upper and lower limb segments.

In the remainder of this work, we move on to refine this application case of human

upper limb motion capture. Through the definition of a formal calibration methodology,

81

experimental protocol, and analysis methods, we improve the functionality of the current

application prototype.

82

4 Mathematical Methods

The mathematical theory in this work ranges from the basic general

representations of spatial position level kinematics to the development of a particular

calibration framework that enables us to reconstruct a subject’s upper limb in a virtual world

with registration of physical and virtual fixtures. We begin this section by covering the basics

of the spatial kinematics theory needed in this work and the general methods needed when

working with raw IMU data. Then we develop the forward and inverse kinematics models of

the subject’s upper limb and apply these models to a calibration procedure, experimental

protocol, and experimental data analysis methods.

4.1 Coordinate Frames, Vectors, and Rotations

A coordinate frame is defined by a six degree of freedom translation and rotation

pair with respect to some other coordinate frame. The base coordinate frame in this work is

taken to be the fixed frame 𝐹. This coordinate frame is special since its basis vectors will be

the set of standard basis vectors 𝒆𝑥 𝒆𝑦 and 𝒆𝑧 given numerically (4-1).

𝒆𝑥 = [100] 𝒆𝑦 = [

010] 𝒆𝑧 = [

001]

(4-1)

All other coordinate frames are defined with respect to another coordinate frame,

such as this frame 𝐹 by the pair of a translational displacement vector and a rotation matrix

that describes the relative orientation between the two frames. We may consider these to be

𝒅𝐵𝐹 and 𝑹𝐵

𝐹 , respectively, for some arbitrary frame 𝐵 . The displacement vector 𝒅𝑖𝑗

is the

83

vector drawn from the origin of frame 𝑗 to the origin of frame 𝑖 and the rotation matrix 𝑹𝑖𝑗 is

the transformation from frame 𝑖 to frame 𝑗.

When the context demands that vectors be componentized, it is assumed that

components are taken in the fixed frame 𝐹 unless a left superscript is given to the variable.

For example, the following relationships in (4-2) show the symbolic form of a vector 𝒅𝑖𝑗 with

components taken in frame 𝑘 . The convention when the left superscript is absent is to

assume that 𝑘 = 𝑗 as is often the case.

𝒌𝒅𝑖

𝑗, 𝒅𝑖

𝑗≡ 𝒋𝒅𝑖

𝑗

(4-2)

Vector operations such as the dot product and cross product are performed in the

typical way, however the cross product is of particular interest in that we define a new

operator [(⋅)̃] for one chosen representation of this operation. The cross product of two

vectors 𝒖 and 𝒗 is represented as the multiplication of a skew symmetric matrix [�̃�] with the

original vector 𝒗 as shown in (4-3).

𝒖 × 𝒗 ≡ [�̃�]𝒗

with,

[�̃�] = [

0 −𝑢𝑧 𝑢𝑦𝑢𝑧 0 −𝑢𝑥−𝑢𝑦 𝑢𝑥 0

]

(4-3)

A unit quaternion is given by a scalar part 𝑠 and vector part 𝒗 so that the

quaternion 𝒒 is given in (4-4) along with its conjugate �̅�.

84

𝒒 = [𝑠𝒗] , �̅� = [

𝑠−𝒗]

(4-4)

We define multiplication on two quaternions 𝒒𝑙 and 𝒒𝑟 (the left and right

operands) to produce 𝒒𝑝 as shown in (4-5).

𝒒𝑝 = 𝒒𝑙𝒒𝑟

= [𝑠𝑙𝑠𝑟 − 𝒗𝑙

T𝒗𝑟𝑠𝑙𝒗𝑟 + 𝑠𝑟𝒗𝑙 + [�̃�𝑙]𝒗𝑟

]

(4-5)

The rotation of a vector 𝒑 to its image 𝒑′ by a quaternion 𝒒 can be computed as in

(4-6) by applying the conjugation function of 𝒒 to a new quaternion with the vector part

given by 𝒑 and a zero scalar part.

[0𝒑′] = 𝒒 [

0𝒑] �̅�

(4-6)

The rotation matrix 𝑹 that performs this same operation on 𝒑 is calculated from

𝒒 as shown in (4-7). We derive this formula for 𝑹 by rotating each of the standard basis

vectors by 𝒒 as follows.

85

[0

𝑹(⋅,1)] = 𝒒 [

0𝒆𝑥] �̅�, [

0𝑹(⋅,2)

] = 𝒒 [0𝒆𝑦] �̅� , [

0𝑹(⋅,3)

] = 𝒒 [0𝒆𝑧] �̅�

and,

𝑹(𝒒) = [𝑹(⋅,1) 𝑹(⋅,2) 𝑹(⋅,3)]

= [

𝑠2 + 𝑣𝑥2 − 𝑣𝑦

2 − 𝑣𝑧2 2(𝑣𝑥𝑣𝑦 − 𝑠𝑣𝑧) 2(𝑣𝑥𝑣𝑧 + 𝑠𝑣𝑦)

2(𝑣𝑥𝑣𝑦 + 𝑠𝑣𝑧) 𝑠2 − 𝑣𝑥2 + 𝑣𝑦

2 − 𝑣𝑧2 2(𝑣𝑦𝑣𝑧 − 𝑠𝑣𝑥)

2(𝑣𝑥𝑣𝑧 − 𝑠𝑣𝑦) 2(𝑣𝑦𝑣𝑧 + 𝑠𝑣𝑥) 𝑠2 − 𝑣𝑥2 − 𝑣𝑦

2 + 𝑣𝑧2

]

= (𝑠2 − 𝒗T𝒗)𝑰3×3 + 2(𝒗𝒗T + 𝑠[�̃�])

(4-7)

Then substituting the unit norm constraint on 𝒒, namely 1 = 𝑠2 + 𝒗T𝒗, allows us

to rewrite the first term of the final line in (4-7) to rewrite this conversion formula in its final

form shown here in (4-8).

𝑹(𝒒) = (1 − 2𝒗T𝒗)𝑰3×3 + 2(𝒗𝒗

T + 𝑠[�̃�]) (4-8)

4.2 Working with Sensor Data

The nature of IMU data that we receive from the sensors in both Myo and Sphero

are subject to a few similar characteristics. These are explained in detail with respect to use

of the data for kinematics modeling and analysis. The position level data, an estimated

quaternion, must be calibrated by capturing a datum for rotational offset that is to be

removed in future realizations of data. The vector data, from gyroscope and accelerometer

data, must also be interpreted with its components in the correct reference frame. And

finally, the measured acceleration from the accelerometer can conveniently be adjusted to

calculate the kinematic acceleration.

86

4.2.1 Setting Home Pose

The IMU estimated orientation data must initially be calibrated against a known

orientation before its absolute orientation can be calculated at a later time. This is due to the

fact that the estimation algorithms are initialized with an unknown inertial reference frame

at an unknown time. The initial inertial reference frame 𝑁𝑠 for a sensor frame 𝑠 encodes very

little information about the physical world. All that is known about 𝑁𝑠 is that its vertical basis

vector 𝒛𝑁𝑠 is antiparallel to gravity. Otherwise, this reference frame, and thus the orientation

of the sensor, is unknown with respect to any rotation about the gravity vector. For this

reason, the following procedure is necessary in use cases when the absolute orientation of

the sensor is desired.

The general procedure used to set the home pose for a sensor is simply described

as one in which we capture the orientation of the sensor during a known physical orientation

so that it can be removed as a sort of rotational offset in future realizations of the data. We

compute the relationship between the raw sensor orientation 𝑹𝑠𝑁𝑠 and the calibrated sensor

orientation in the fixed frame 𝑹𝑠𝐹 by capturing offsets 𝑹𝐹

𝑁𝑠 simultaneously and applying the

following loop closure equation.

𝑹𝑠𝑁𝑠 = 𝑹𝐹

𝑁𝑠𝑹𝑠𝐹

(4-9)

Each offset 𝑹𝐹𝑁𝑠 is dependent upon a definition of the home pose for the sensor in

the fixed frame �̃�𝑠𝐹 which is chosen to be identity in this work. The offsets are computed by

solving the preceding loop closure equation with 𝑹𝑠𝐹 = �̃�𝑠

𝐹.

87

𝑹𝐹𝑁𝑠 = 𝑹𝑠

𝑁𝑠(�̃�𝑠𝐹)T= 𝑹𝑠

𝑁𝑠𝑰3×3 = 𝑹𝑠𝑁𝑠

(4-10)

Subsequent raw sensor orientation data is then interpreted with respect to the

fixed frame by solving the rotational loop closure equation for 𝑹𝑠𝐹 and applying the offsets

captured during home pose calibration.

𝑹𝑠𝐹 = (𝑹𝐹

𝑁𝑠)T𝑹𝑠𝑁𝑠

(4-11)

4.2.2 Vector Data Interpretation

The IMU sensors also contain data from gyroscope and accelerometer sources that

must be interpreted properly for effective use. In addition to understanding the physical

meaning of the data, we must have an awareness of how to work with this data between the

various relevant coordinate systems.

The raw gyroscope and accelerometer data from both Myo and Sphero bears

characteristics that are typical for IMU data. Both sources are reported, after having been

measured, in the rotating body frame 𝑠 of the sensor 𝑠. The gyroscope data is as expected the

angular velocity vector 𝑠𝝎 with components taken in the 𝑠 frame and units of degrees per

second. Describing the accelerometer data in terms of kinematic quantities is slightly more

involved.

The nature of measuring accelerations involves relying upon the indirect

observation of acceleration through computing the force due to both acceleration and gravity

acting on a proof mass. The effect of this is that the measured acceleration 𝑠�̂� is a summation

88

of the kinematic acceleration 𝒂 and the acceleration due to gravity 𝒈. We denote the gravity

vector in inertial coordinates to be given by (4-12).

𝑁𝑠𝒈 = −𝑔𝒆𝑧

(4-12)

The measured acceleration is then equal to the combination of kinematic

acceleration and apparent acceleration due to gravity as shown in (4-13).

𝑠�̂� = 𝑠𝒂 − 𝑠𝒈

(4-13)

Fortunately with both Myo and Sphero we have access to an estimated quaternion

relating the 𝑠 and 𝑁𝑠 frames from which we can compute the rotation matrix 𝑹𝑠𝑁𝑠 . This

rotation matrix along with the definition of the gravity vector in (4-12) can be applied to

(4-13) to solve for the kinematic acceleration of the sensor with components in the inertial

frame 𝑁𝑠 shown here.

𝑁𝑠𝒂 = 𝑁𝑠�̂� + 𝑁𝑠𝒈

= 𝑹𝑠𝑁𝑠 𝑠�̂� − 𝑔𝒆𝑧

(4-14)

An alternative perspective on the kinematic acceleration that is simpler than

(4-14) while losing some information can be found in looking only at its magnitude. The

computation of kinematic acceleration magnitude is performed by taking the norm of the

measured acceleration and subtracting the magnitude of acceleration due to gravity.

‖𝒂‖ = ‖�̂�‖ − 𝑔

(4-15)

89

The interpretation of the sensor angular velocity is straightforward in that the

vector can be taken into the inertial frame by premultiplication of the rotation matrix.

𝑁𝑠𝝎 = 𝑹𝑠

𝑁𝑠 𝑠𝝎 (4-16)

4.3 Upper Limb Kinematics

The home calibrated sensor data can be used to represent the subject’s upper limb

motion in multiple ways. The data can be used directly with a forward kinematics model of

the upper limb. This is the way that we represent the subject’s limb through the calibration

process in the following section. Alternatively, we can map the sensor data back onto a set of

scalar joint angles through an inverse kinematics calculation. Finally, a variation of the

forward kinematics representation can be computed on the result from inverse kinematics.

4.3.1 Forward Kinematics

The forward kinematics model that we adopt relies strictly upon the estimated

quaternion data from both Myo devices and Sphero. Our goal here is to determine the

position of a point of interest on the subject’s upper limb by using only data from the sensors.

These calculations will then enable us to perform calibration of the kinematic model within

its task environment in the following section.

In order to relate the sensor data to the human upper limb, we make a critical

assumption. We assume that each sensor is rigidly attached to its respective limb so that the

orientation of the sensor is exactly the orientation of the limb at the point of attachment

between the sensor and the limb. With this assumption, we can now think about the

orientation of three segments of the upper limb as directly known from the data.

90

The three segments of the upper limb are taken to be represented by three

coordinate frames 𝑈 𝐿 and 𝐻 attached to the upper arm, lower arm and hand, respectively.

Each of these sensor frames is attached to the limb so that the 𝒙 axis points in the lateral

direction and the 𝒛 axis points vertically upward when the subject assumes the t-pose with

the arm extended laterally and palm facing down.

The connectivity between these segments is modeled only by spherical joints in

at this stage since we’re relying only upon the sensor data information. In this case, the so-

called sensor joint variables are represented by the rotation matrices 𝑹𝑈 𝑹𝐿 and 𝑹𝐻. Note

that for the remainder of this work, when a rotation matrix lacks a superscript reference

frame, it is to be taken with respect to the fixed frame 𝐹.

With the inclusion of four geometrical parameters, three arm segment lengths 𝑙𝑈,

𝑙𝐿, 𝑙𝐻 and the radius of Sphero 𝑟𝑆, we can now fully specify the mapping of sensor joint data

onto the location of Sphero’s center point in the fixed frame 𝒅𝑆𝐹 . Of course, this formulation

involves one more assumption that the center of Sphero exists vertically downward from the

origin of frame 𝐻 when the subject is in home position. So long as Sphero does not move with

respect to the subject’s wrist point as viewed in the hand frame 𝐻 , this is a reasonable

assumption.

The mathematical expression of this forward kinematics formulation is aided by

the visual depiction of Figure 4-1 and is given by the following vector loop closure equation.

𝒅𝑆𝐹 = 𝒍𝑈 + 𝒍𝐿 + 𝒍𝐻 + 𝒓𝑆

(4-17)

The point of interest is the center of Sphero located by 𝒅𝑆𝐹 , and the vectors 𝒍(⋅) and

𝒓𝑆 point along the 𝒙(⋅) and 𝒛𝐻 basis vectors, respectively. This formula can be represented

91

more directly in terms of the sensor data rotation matrices and the geometrical parameters

by writing it in terms of the parameters’ scalar magnitudes as shown in (4-18).

Figure 4-1: Upper limb forward kinematics model

𝒅𝑆𝐹 = 𝑹𝑈𝒆𝑥𝑙𝑈 +𝑹𝐿𝒆𝑥𝑙𝐿 + 𝑹𝐻𝒆𝑥𝑙𝐻 − 𝑹𝐻𝒆𝑧𝑟𝑠

(4-18)

Finally, we notice that the sensor data must be homed before it can be used to

compute the forward kinematics. This is performed as described above in (4-10) by first

capturing the rotational offsets during performance of the t-pose using (4-19).

𝑹𝐹𝑁𝑈 = 𝑹𝑈

𝑁𝑈

𝑹𝐹𝑁𝐿 = 𝑹𝐿

𝑁𝐿

𝑹𝐹𝑁𝐻 = 𝑹𝐻

𝑁𝐻⏟ during home pose

(4-19)

After these rotational offsets are recorded, they are to be applied to all future

sensor data in order to transform it to the common fixed frame 𝐹 as described generally in

(4-11) and shown specifically here in (4-20).

92

𝑹𝑈 = 𝑹𝑈𝐹 = (𝑹𝐹

𝑁𝑈)T𝑹𝑈𝑁𝑈

𝑹𝐿 = 𝑹𝐿𝐹 = (𝑹𝐹

𝑁𝐿)T𝑹𝐿𝑁𝐿

𝑹𝐻 = 𝑹𝐻𝐹 = (𝑹𝐹

𝑁𝐻)T𝑹𝐻𝑁𝐻

(4-20)

4.3.2 Inverse Kinematics

As a final step in representing a model of human upper limb motion, we may wish

to map the rotation data for each limb segment into a minimal parameter description of the

joint motion described by a set of scalar rotation angles. To reason about this process at a

conceptual level higher than simple geometry, we leverage two Euler angle sets to calculate

the joint angles in a systematic way. The inverse Euler angle mapping is then applied to the

three relative rotation matrices between adjacent limb segments. Then, this particular nine

parameter description is reduced to an assumed seven degrees of freedom by introducing

two constraints on the parameter set.

Euler angle parameterization is given by an ordered sequence of three rotations

performed about the principal axes. The sequence is denoted 𝑖 − 𝑗 − 𝑘 to specify the axes of

rotation about which the proximal frame 𝑁 is rotated through the angles 𝜃1 𝜃2 and 𝜃3 ,

respectively, to result in the distal frame 𝐷 . Each successive rotation is performed with

respect to the coordinate frame resulting from the previous rotation. The composed rotation

matrix is given by (4-21).

𝑹𝐷𝑁 = 𝑴𝑖(𝜃1)𝑴𝑗(𝜃2)𝑴𝑘(𝜃3) = 𝑴𝑖𝑗𝑘

(4-21)

Where 𝑖 𝑗 and 𝑘 can be any of 𝑥 𝑦 or 𝑧, and the matrices 𝑴𝑥(𝜃) 𝑴𝑦(𝜃) and 𝑴𝑧(𝜃)

are rotation transformations about the principal axes shown in (4-22).

93

𝑴𝑥(𝜃) = [1 0 00 𝑐𝜃 −𝑠𝜃0 𝑠𝜃 𝑐𝜃

]

𝑴𝑦(𝜃) = [𝑐𝜃 0 𝑠𝜃0 1 0−𝑠𝜃 0 𝑐𝜃

]

𝑴𝑧(𝜃) = [𝑐𝜃 −𝑠𝜃 0𝑠𝜃 𝑐𝜃 00 0 1

]

(4-22)

In this work, we leverage two nonsymmetric parameterizations to perform the

decomposition of the joint rotation matrices into rotations about the assumed joint axes. The

𝑧 − 𝑦 − 𝑥 and 𝑥 − 𝑦 − 𝑧 parameterizations will be used and derived in the remainder of this

section. Applying (4-21) and (4-22) to these sequences allows us to calculate the

compositions 𝑴𝑥𝑦𝑧 and 𝑴𝑧𝑦𝑥 as shown in (4-23).

𝑴𝑥𝑦𝑧 = [

𝑐2𝑐3 −𝑐2𝑠3 𝑠2𝑐1𝑠3 + 𝑠1𝑠2𝑐3 𝑐1𝑐3 − 𝑠1𝑠2𝑠3 −𝑠1𝑐2𝑠1𝑠3 − 𝑐1𝑠2𝑐3 𝑠1𝑐3 + 𝑐1𝑠2𝑠3 𝑐1𝑐2

]

𝑴𝑧𝑦𝑥 = [

𝑐1𝑐2 −𝑠1𝑐3 + 𝑐1𝑠2𝑠3 𝑠1𝑠3 + 𝑐1𝑠2𝑐3𝑠1𝑐2 𝑐1𝑐3 + 𝑠1𝑠2𝑠3 −𝑐1𝑠3 + 𝑠1𝑠2𝑐3−𝑠2 𝑐2𝑠3 𝑐2𝑐3

]

(4-23)

Inversion of these composed rotation matrices will enable us to determine joint

angles between adjacent upper limb segments. We’ll denote the elements of the matrices

𝑴(⋅,⋅,⋅) = [𝑚𝑖𝑗] by their subscripts as 𝑚𝑖𝑗 in the following derivation of Euler angle inversion

formulas shown in (4-24) and (4-25).

94

𝜃1 = atan2(−𝑚23, 𝑚33) = atan2(𝑠1𝑐2, 𝑐1𝑐2)

𝜃2 = asin(𝑚13) = asin(𝑠2)

𝜃3 = atan2(−𝑚12,𝑚11) = atan2(−𝑐2𝑠3, −𝑐2𝑐3)⏟ 𝑥−𝑦−𝑧

(4-24)

𝜃1 = atan2(𝑚21,𝑚11) = atan2(𝑠1𝑐2, 𝑐1𝑐2)

𝜃2 = asin(−𝑚31) = asin(𝑠2)

𝜃3 = atan2(𝑚32,𝑚33) = atan2(𝑐2𝑠3, 𝑐2𝑐3)⏟ 𝑧−𝑦−𝑥

(4-25)

The inverse Euler angle formulas in (4-24) and (4-25) are applied to the relative

rotations between segments 𝑹𝑈𝐹 𝑹𝐿

𝑈 and 𝑹𝐻𝐿 with alternate variable names assigned to this

representation of the shoulder, elbow, and wrist joint rotations given in Table 4-1. These

variable names are used to disambiguate the numbered Euler angles presented previously.

Table 4-1: Inverse kinematics joint variable definitions

Joint 𝒊 − 𝒋 − 𝒌 𝑴𝒊𝒋𝒌 𝜽𝟏 𝜽𝟐 𝜽𝟑

Shoulder 𝑧 − 𝑦 − 𝑥 𝑹𝑈𝐹 𝜃𝑠𝑧 𝜃𝑠𝑦 𝜃𝑠𝑥

Elbow 𝑧 − 𝑦 − 𝑥 𝑹𝐿𝑈 𝜃𝑒𝑧 𝜃𝑒𝑦 𝜃𝑒𝑥

Wrist 𝑥 − 𝑦 − 𝑧 𝑹𝐻𝐿 𝜃𝑤𝑥 𝜃𝑤𝑦 𝜃𝑤𝑧

The interpretation of these nine parameters such that they accurately represent

the pose of a seven degree of freedom human arm are subject to two additional pieces of

information. First, we notice that the angles 𝜃𝑒𝑥 and 𝜃𝑤𝑥 are rotations about the same axis.

These two variables represent the contributions from two sensor sources 𝑹𝐿𝑈 and 𝑹𝐻

𝐿 to a

single physical joint rotation of the lower arm 𝜙𝑒𝑥 that is calculated by the sum in (4-26).

𝜙𝑒𝑥 = 𝜃𝑒𝑥 + 𝜃𝑤𝑥

(4-26)

95

The remaining extra degree of freedom in our parameterization is superficial with

respect to the physical motion of the human upper limb. We assume that the rotation of the

elbow is a rotation purely about the 𝑧 and 𝑥 axes. Thus, any nonzero value of the variable 𝜃𝑒𝑦

represents error in the determined pose because of our assumption in (4-27).

𝜃𝑒𝑦 = 0

(4-27)

The joint variables used to represent the upper limb pose are then taken to be 𝜃𝑠𝑧

𝜃𝑠𝑦 𝜃𝑠𝑥 𝜃𝑒𝑧 𝜙𝑒𝑥 𝜃𝑤𝑦 and 𝜃𝑤𝑧 . After mapping the joint rotations into this set of joint variables,

we must check the validity of the assumption in (4-27) to ensure that excessive error doesn’t

exist in this pose representation.

Reconstruction of the forward kinematics can then be computed using the results

from inverse kinematics. One must use the formulas in (4-23) to compute 𝑹𝐿𝑈 with 𝜃𝑒𝑦 = 0

and then use this matrix to compute 𝑹𝐿𝐹 before continuing on to compute the forward

kinematics using joint rotation matrices as previously described. The validity of the joint

angles and the inverse kinematics motion model can be checked by comparing this

reconstructed forward kinematics result with the result computed from the sensor data

directly by computing the distance between the calculated 𝒅𝑆𝐹 from each method.

4.4 Calibration Problem

After having removed rotational offsets from the sensor data and then mapping

the data through the forward kinematics calculation, we realize the need for calibration.

There are two problems that this calibration is set to solve. First, the geometric parameters

of the upper limb needed in order to compute the forward kinematics equation are not

96

known. We must devise some method to estimate these parameters before they can be used

reliably. Second, the fixed frame defined at the moment the home pose is set has little

relationship to the physical world. Supposing that we desire to relate the kinematics of the

subjects are to the location of objects in the physical world, it is critical to determine the rigid

body transformation between the fixed frame 𝐹 and some known coordinate system of

interest in the task environment.

The proposed calibration method involves the use of a calibration jig in which

calibration point fixtures are installed at known relative locations as depicted in Figure 4-2.

The calibration point fixtures are designed to mechanically interface Sphero so that its center

point remains invariant under spherical rotation. With many data realizations captured

while the subject is holding Sphero at each of multiple calibrations points, we estimate the

upper limb geometric parameters and the calibration point locations in one optimization

step.

Figure 4-2: Calibration point definitions

The use of three calibration points 𝐶1 𝐶2 and 𝐶3 provides enough information to

reconstruct a rigid body transformation between the fixed frame 𝐹 and a new task frame 𝑇

that is embedded in the calibration jig. Figure 4-2 shows the three calibration points and

their locating vectors with respect to the fixed frame origin and with components taken in

the fixed frame. The treatment of constraints on the calibration points is the additional

97

information that allows us to hone in on the optimal values for the upper limb geometrical

parameters and the calibration point locations.

As we develop the constraints on the calibration points, we will need to assume

some compatible configuration of the calibration points within a calibration jig. Figure 4-3

depicts the three calibration points along with four vectors that are computed from the

locations of the calibration points. There are three relative displacement vectors and one

vector that’s normal to the plane spanned by these relative displacement vectors.

Constraints on the geometry of the calibration points will be applied directly to the

components of these vectors taken in the fixed frame 𝐹.

Figure 4-3: Calibration point calculated vectors

Some examples of the constraints we will develop from these calculated vectors

include enforcing known vector magnitude, known vector component values, vector

orthogonality. The latter is a property that is exploited in efforts to simplify the definition of

a unique coordinate frame in the task space 𝑇 as depicted in Figure 4-4.

Figure 4-4: Choice of task space coordinate frame

98

We choose to define the task frame 𝑇 directly from the relative displacement

vectors with the origin’s position and basis vector given with respect to the fixed frame by

(4-28).

𝒅𝑇𝐹 = 𝒅𝐶2

𝐹

𝒙𝑇 =𝒓𝐶2/𝐶3𝑟𝐶2/𝐶3

𝒚𝑇 =𝒓𝐶2/𝐶1𝑟𝐶2/𝐶1

𝒛𝑇 =𝒏𝑇𝑛𝑇

=𝒓𝐶2/𝐶3 × 𝒓𝐶2/𝐶1‖𝒓𝐶2/𝐶3 × 𝒓𝐶2/𝐶1‖

(4-28)

In the remainder of this section we develop the optimization framework including

formulation of the objective function and the geometrical constraints followed by a

discussion of solving the problem computationally.

4.4.1 Objective Function

The objective of this calibration optimization is to minimize the error between the position

𝒅𝑆𝐹 of center of Sphero computed from the forward kinematics of (4-18) and the location of

each 𝑖-th calibration point 𝒅𝐶𝑖𝐹 to determine the lengths of each upper limb segment and the

location of the calibration points. Figure 4-5 shows the definition of the error vector 𝝐 =

𝒅𝐶𝑖𝐹 − 𝒅𝑆

𝐹 that we will minimize over the design variables 𝒙 shown in (4-29).

99

Figure 4-5: Calibration objective function error vector

𝒙 = [𝒍T (𝒅𝐶1𝐹 )

T(𝒅𝐶2

𝐹 )T(𝒅𝐶3

𝐹 )T]T

, 𝒍 = [𝑙𝑈 𝑙𝐿 𝑙𝐻]T

(4-29)

We begin the formulation of the objective function by considering only the 𝑘-th

realization for the 𝑖-th calibration point, substituting 𝝐 into (4-18), and factoring out the

variables in 𝒙 while isolating 𝝐.

𝒅𝑆𝐹 = 𝑹𝑈𝒆𝑥𝑙𝑈 + 𝑹𝐿𝒆𝑥𝑙𝐿 + 𝑹𝐻𝒆𝑥𝑙𝐻 − 𝑹𝐻𝒆𝑧𝑟𝑠−𝝐 = 𝑹𝑈𝒆𝑥𝑙𝑈 + 𝑹𝐿𝒆𝑥𝑙𝐿 + 𝑹𝐻𝒆𝑥𝑙𝐻 − 𝒅𝐶𝑖

𝐹 − 𝑹𝐻𝒆𝑧𝑟𝑠

−𝝐 = [𝑹𝑈𝒆𝑥 𝑹𝐿𝒆𝑥 𝑹𝐻𝒆𝑥 −𝑰3×3]

[ 𝑙𝑈𝑙𝐿𝑙𝐻𝒅𝐶𝑖𝐹]

− 𝑹𝐻𝒆𝑧𝑟𝑠

−𝝐 = [𝑨𝑘𝑖 −𝑰3×3] [𝒍𝒅𝐶𝑖𝐹 ] − 𝒃𝑘𝑖

(4-30)

We can write (4-30) 𝑃𝑖-many times by expanding along the 𝑘 dimension to obtain

the result in (4-31).

100

[ 𝑨1𝑖 −𝑰⋮ ⋮𝑨𝑘𝑖 −𝑰⋮ ⋮𝑨𝑃𝑖𝑖 −𝑰]

[𝒍𝒅𝐶𝑖𝐹 ] −

[ 𝒃1𝑖⋮𝒃𝑘𝑖⋮𝒃𝑃𝑖𝑖]

= −

[ 𝝐1𝑖⋮𝝐𝑘𝑖⋮𝝐𝑃𝑖𝑖]

[𝑨𝐾𝑖 −𝑰3𝑃𝑖×3] [𝒍𝒅𝐶𝑖𝐹 ] − 𝒃𝐾𝑖 = −𝝐𝐾𝑖

(4-31)

We can then write (4-31) 𝑀-many times by expanding along the 𝑖 dimension for

each of 𝑀 = 3 calibration points to obtain the global matrices shown in (4-32).

[

𝑨𝐾1 −𝑰3𝑃1×3 0 0

𝑨𝐾2 0 −𝑰3𝑃2×3 0

𝑨𝐾3 0 0 −𝑰3𝑃3×3

]

[ 𝒍𝒅𝐶1𝐹

𝒅𝐶2𝐹

𝒅𝐶3𝐹]

− [

𝒃𝐾1𝒃𝐾2𝒃𝐾3

] = − [

𝝐𝐾1𝝐𝐾2𝝐𝐾3]

𝑨𝐺𝒙 − 𝒃𝐺 = −𝝐𝐺

(4-32)

Finally, we complete the formulation of the objective function by calculating the

squared norm of the error 𝝐T𝝐 to obtain 𝑓(𝒙) and then calculate the gradient ∇𝑓(𝒙) in (4-33).

𝑓(𝒙) = 𝒙T𝑨𝐺

T𝑨𝐺𝒙 − 2𝒃𝐺T𝑨𝐺𝒙 + 𝒃𝐺

T𝒃𝐺 = 𝝐𝐺T𝝐𝐺

∇𝑓(𝒙) = 2𝑨𝐺T𝑨𝐺𝒙 − 2𝒃𝐺

T𝑨𝐺𝒙

(4-33)

4.4.2 Design Variable Bounds

The geometrical parameters in 𝒍 must necessarily be positive values since they

represent the lengths of the subject’s virtual upper limb as defined in the kinematic model

assumed in this work. If left unconstrained, valid solutions may be obtained through various

possible combinatorial permutations of the vector loop equation in (4-18). However, to

avoid ambiguity with future interpretation of the calibration result, it’s necessary to impose

101

a bound constraint that the link length parameters be non-negative as shown in (4-34). Note

that the calibration point locations 𝒅𝐶𝑖𝐹 are free vectors and so their component magnitudes

are unconstrained on ℛ3.

𝒈𝐵 = −𝒍 ≼ 𝟎

(4-34)

4.4.3 Calibration Point Distance Constraints

The first reasonable constraints to impose on the calibration points are the

relative distances between each pair of the three calibration points. We easily compute the

relative displacement vectors as differences of elements in the design vector. The squared

norm of these vectors is a value that we know from the geometry of the calibration jig, and

it’s invariant under rigid transformations of the task space. We proceed to compute the norm

of these relative displacement vectors and then cast the expression into quadratic form.

The three relative displacement vectors are taken to be 𝒓𝐶2/𝐶1 𝒓𝐶2/𝐶1 𝒓𝐶2/𝐶1, and

they are calculated by (4-35).

𝒓𝐶2/𝐶1 = 𝒅𝐶2

𝐹 − 𝒅𝐶1𝐹

𝒓𝐶2/𝐶3 = 𝒅𝐶2𝐹 − 𝒅𝐶3

𝐹


𝐹

(4-35)

The form of these quadratic equality constraints is given by the following.

ℎ𝐷21 = (𝒓𝐶2/𝐶1)T𝒓𝐶2/𝐶1 − (𝑟𝐶2/𝐶1)

2= 0

ℎ𝐷23 = (𝒓𝐶2/𝐶3)T𝒓𝐶2/𝐶3 − (𝑟𝐶2/𝐶3)

2= 0

ℎ𝐷31 = (𝒓𝐶3/𝐶1)T𝒓𝐶3/𝐶1 − (𝑟𝐶3/𝐶1)

2= 0

(4-36)

102

Next we substitute (4-35) into (4-36) and expand the product to obtain three

quadratic equations.

ℎ𝐷21 = (𝒅𝐶2𝐹 )

T𝒅𝐶2𝐹 − 2(𝒅𝐶2

𝐹 )T𝒅𝐶1𝐹 + (𝒅𝐶1

𝐹 )T𝒅𝐶1𝐹 − (𝑟𝐶2/𝐶1)

2

ℎ𝐷23 = (𝒅𝐶2𝐹 )




2

ℎ𝐷31 = (𝒅𝐶3𝐹 )




2

(4-37)

Finally, we can represent the three constraint equations in (4-37) in quadratic

matrix vector form by introducing the matrices 𝑸21 𝑸23 and 𝑸31 shown in (4-38).

𝑸21 = 2 [

𝟎3×3 … ⋮ 𝑰3×3 −𝑰3×3 −𝑰3×3 𝑰3×3 ⋮ … 𝟎3×3

]

12×12

𝑸23 = 2 [

𝟎3×3 … ⋮ 𝟎3×3 … ⋮ 𝑰3×3 −𝑰3×3 −𝑰3×3 𝑰3×3

]

12×12

𝑸31 = 2 [

𝟎3×3 … ⋮ 𝑰3×3 ⋮ −𝑰3×3 … 𝟎3×3 … −𝑰3×3 ⋮ 𝑰3×3

]

12×12

(4-38)

The final form of these constraints is shown in (4-39) along with the constraints’

respective analytical gradients.

103

ℎ𝐷21 =1

2𝒙T𝑸21𝒙 − (𝑟𝐶2/𝐶1)

2

∇ℎ𝐷21 = 𝑸21𝒙

ℎ𝐷23 =1

2𝒙T𝑸23𝒙 − (𝑟𝐶2/𝐶3)

2

∇ℎ𝐷23 = 𝑸23𝒙

ℎ𝐷31 =1

2𝒙T𝑸31𝒙 − (𝑟𝐶3/𝐶1)

2

∇ℎ𝐷31 = 𝑸31𝒙

(4-39)

4.4.4 Calibration Point Orthogonality Constraint

The calibration points will be used directly to construct the basis for the task

frame 𝑇, so we also constrain two of the calibration points’ relative position vectors to be

orthogonal. We compute the dot product of two calibration point relative displacement

vectors, and then constraint this value to be equal to zero. In this way, we obtain a new

quadratic equality constraint.

The two relative displacement vectors we work with here are taken to be 𝒓𝐶2/𝐶3

and 𝒓𝐶2/𝐶1 .

𝒓𝐶2/𝐶3 = 𝒅𝐶2

𝐹 − 𝒅𝐶3𝐹


𝐹

(4-40)

The form of these quadratic equality constraints is given by the following dot

product set equal to zero.

ℎ𝑂 = (𝒓𝐶2/𝐶3)T𝒓𝐶2/𝐶1 = 0

(4-41)

104

Next we substitute (4-40) into (4-41) and expand the product to obtain the

following quadratic equation.

ℎ𝑂 = (𝒅𝐶2𝐹 )

T𝒅𝐶2𝐹 − (𝒅𝐶2

𝐹 )T𝒅𝐶1𝐹 − (𝒅3

𝐹)T𝒅𝐶2𝐹 + (𝒅3

𝐹)T𝒅𝐶1𝐹

(4-42)

We can represent the quadratic orthogonality constraint equation in (4-42) by

introducing the matrix 𝑸𝑂 .

𝑸𝑂 = [

𝟎3×3 … ⋮ 𝟎3×3 −𝑰3×3 𝑰3×3 −𝑰3×3 2𝑰3×3 −𝑰3×3 𝑰3×3 −𝑰3×3 𝟎3×3

]

(4-43)

The final form of this constraint equation is shown in (4-44) along with its

analytical gradient.

ℎ𝑂 =1

2𝒙T𝑸𝑂𝒙

∇ℎ𝑂 = 𝑸𝑂𝒙

(4-44)

4.4.5 Calibration Point Planar Constraints

The calibration jig will also be designed so that the two orthogonal relative

displacement vectors will exist in the horizontal plane. In fact, this condition is satisfied

exactly when all three calibration points exist in the horizontal plane. One great benefit of

employing this constraint is that it enables to encoding of such configuration information

into the form of a linear equality constraint. All that must be done is to set the vertical

component of the calculated relative displacement vectors to zero.

105

We begin with the desired linear form for this set of linear equality constraints as

shown in (4-45).

𝒉𝑃 = 𝑨𝑃𝒙 − 𝒃𝑃 = 𝟎2×1

(4-45)

Next, we write down the coefficients in 𝑨𝑃 so that 𝑨𝑃𝒙 calculates a 2 × 1 column

vector containing the vertical components of 𝒓𝐶2/𝐶3 = 𝒅𝐶2𝐹 − 𝒅𝐶3

𝐹 and 𝒓𝐶2/𝐶1 = 𝒅𝐶2𝐹 − 𝒅𝐶1

𝐹 and

realize that 𝒃𝑃 is the zero vector.

𝑨𝑃 = [𝟎1×3 0 0 0 0 0 1 0 0 −1𝟎1×3 0 0 −1 0 0 1 0 0 0

]

𝒃𝑃 = 𝟎2×1

(4-46)

The linear equality constraint that enforces that the calibration points exist in the

horizontal plane is given by (4-45) and (4-46). Since these constraints are linear, the gradient

is constant and is given entirely by the coefficient matrix 𝑨𝑃.

4.4.6 Calibration Point Normal Vector Constraints

The final set of constraints we may impose on the calibration points involves

properties of the vector 𝒏𝑇 that is normal to the relative displacement vectors we used

previously. We compute 𝒏𝑇 by a cross product and realize that the three components can be

constrained in a few different ways. The horizontal components can be constrained to zero

value as an alternative to the planar linear constraints developed previously. The vertical

component can be specified exactly with the expected magnitude and sign. Assuming a

perfect calibration, this would also ensure that 𝒏𝑇 is vertical. But, additionally, this

constraint also serves a very important function to orient the task frame 𝑇. Since another

106

quadratic equality constraint may be burdensome on the optimization solver, we also

formulate a similar inequality constraint using the vertical component of 𝒏𝑇 in which this

value is constrained to be non-negative. In effect, this will allow us to orient the task frame.

The normal vector is taken to be the cross product of the planar relative

displacement vectors used previously. We choose to represent the cross product with the

cross matrix and tilde notation.

𝒏𝑇 = [�̃�𝐶2/𝐶3]𝒓𝐶2/𝐶1

(4-47)

4.4.7 Normal Vector Horizontal Equality Constraints

The equality constraints on the horizontal components of 𝒏𝑇 are given by the

following form in (4-48).

ℎ𝐻𝑥 = (𝒆𝑥)

T𝒏𝑇 = (𝒆𝑥)T[�̃�𝐶2/𝐶3]𝒓𝐶2/𝐶1 = 0

ℎ𝐻𝑦 = (𝒆𝑦)T𝒏𝑇 = (𝒆𝑦)

T[�̃�𝐶2/𝐶3]𝒓𝐶2/𝐶1 = 0

(4-48)

These constraints are compactly represented in quadratic form through the

introduction of matrices 𝑸𝐻𝑥 and 𝑸𝐻𝑦 as shown in (4-49).

107

𝑸𝐻𝑥 =

[ 𝟎3×3 …⋮ 𝟎3×3 −𝑺𝑥 𝑺𝑥

−𝑺𝑥 𝟎3×3 −𝑺𝑥𝑺𝑥 −𝑺𝑥 𝟎3×3]

𝑸𝐻𝑦 =

[ 𝟎3×3 …⋮ 𝟎3×3 −𝑺𝑦 𝑺𝑦

−𝑺𝑦 𝟎3×3 −𝑺𝑦𝑺𝑦 −𝑺𝑦 𝟎3×3]

with,

𝑺𝑥 = [�̃�𝑥] = [0 0 00 0 −10 1 0

]

𝑺𝑦 = [�̃�𝑦] = [0 0 10 0 0−1 0 0

]

(4-49)

And the final formulas for ℎ𝐻𝑥 and ℎ𝐻𝑦 along with their analytical gradients are

given in (4-50).

ℎ𝐻𝑥 =1

2𝒙T𝑸𝐻𝑥𝒙

∇ℎ𝐻𝑥 = 𝑸𝐻𝑥𝒙

ℎ𝐻𝑦 =1

2𝒙T𝑸𝐻𝑦𝒙

∇ℎ𝐻𝑦 = 𝑸𝐻𝑦𝒙

(4-50)

4.4.8 Normal Vector Vertical Equality Constraint

The normal vector vertical equality constraint is given by the following form in

(4-51).

ℎ𝑉 = (𝒆𝑧)

T𝒏𝑇 −𝑟𝐶2/𝐶3𝑟𝐶2/𝐶1 = 0

= (𝒆𝑧)T[�̃�𝐶2/𝐶3]𝒓𝐶2/𝐶1 −𝑟𝐶2/𝐶3𝑟𝐶2/𝐶1 = 0

(4-51)

108

This constraint is written in quadratic form by introducing the 𝑸𝑉 as shown in

(4-52).

𝑸𝑉 = [

𝟎3×3 … ⋮ 𝟎3×3 −𝑺𝑧 𝑺𝑧 −𝑺𝑧 𝟎3×3 −𝑺𝑧 𝑺𝑧 −𝑺𝑧 𝟎3×3

]

with,

𝑺𝑧 = [�̃�𝑧] = [0 −1 01 0 00 0 0

]

(4-52)

The final form of the normal vector vertical component equality constraint is

given along with its gradient in (4-53).

ℎ𝑉 =1

2𝒙T𝑸𝑉𝒙 − 𝑟𝐶2/𝐶3𝑟𝐶2/𝐶1

∇ℎ𝑉 = 𝑸𝑉𝒙

(4-53)

4.4.9 Normal Vector Vertical Inequality Constraint

The relaxation of the normal vector vertical equality constraint into a nonnegative

inequality constraint 𝑔𝑉 is easily given by inspection of ℎ𝑉 from (4-53) along with its

gradient in the following equation (4-54).

𝑔𝑉 = −1

2𝒙T𝑸𝑉𝒙 ≤ 0

∇𝑔𝑉 = −𝑸𝑉𝒙

(4-54)

Note that 𝑸𝑉 is given by the same expression in (4-52).

109

4.4.10 Calibration Constraints Summary

This collection of constraints we have formulated is collected along with some

summary data in Table 4-2. The number of constraints, the linearity of the constraints, and

the type of constraint will be the main focus of the discussion of selecting an appropriate set

of constraints the follows in this section.

Table 4-2: Calibration constraints summary

Function Name Count Linearity Type Equations

𝒈𝐵 Bounds 3 Constant Inequality (4-34)

ℎ𝐷(⋅⋅) Distance 3 Quadratic Equality (4-38), (4-39)

ℎ𝑂 Orthogonality 1 Quadratic Equality (4-43), (4-44)

𝒉𝑃 Planar 2 Linear Equality (4-45), (4-46)

ℎ𝐻(⋅) Normal Vector Horizontal

2 Quadratic Equality (4-49), (4-50)

ℎ𝑉 Normal Vector Vertical

1 Quadratic Equality (4-52), (4-53)

𝑔𝑉 1 Quadratic Inequality (4-52), (4-54)

The constraints we have developed offer some redundancy and overlapping

purpose that allows us to make an informed selection in implementation of a solution to this

optimization problem. However, some of the constraints have no alternative formulation or

may be necessary. We will discuss the latter before comparing options with the former.

The bounds, distance, and orthogonality constraints have no other

representation, and so they must necessarily be used. The bounds are critical in enforcing

110

uniqueness of our kinematic model because they ensure non-negative upper limb segment

lengths. The distance and orthogonality constraints enforce the basic geometry of the

calibration points’ configuration in the calibration jig.

The remaining constraints can then be looked at comparatively as alternative

options. The remaining properties we wish to enforce on our design vector are that the

calibration points lie in a horizontal plane and the orientation of this horizontal plane. We’ll

first consider constraining the calibration points to the horizontal plane. The normal vector

vertical equality constraint could enforce this condition with near-perfect data, but

unfortunately the very errors that we’re minimizing in this optimization can manifest

satisfaction of this constraint without meeting the desired horizontal condition. Therefore

we choose to use a set of two constraints in its place by selecting either the planar constraints

or the normal vector horizontal component constraints. At this point the decision to select

the planar constraints is obvious since they’re expressed with a mathematically simpler

linear function rather than two quadratic expressions.

The final remaining property to enforce on the design variables is the orientation

of the calibration points’ horizontal plane. If we would have chosen to use the normal vector

vertical equality constraint to satisfy the calibration points’ horizontal plane condition, we

would not need to further constrain the problem here because that constraint also enforces

the orientation of the horizontal plane. Since this constraint does also enforce the conditions

imposed by previous constraints (calibration points in the horizontal plane), we choose to

not use this constraint because of risk in over constraining the design space. Instead, the

suitable choice is to use the relaxed version of this constraint by selecting the inequality

constraint variant.

111

We may also find that the assumptions made on the sensors’ estimated quaternion

data may be a bit strict considering possible errors evident in the data. For instance, drifting

of the quaternion estimate may allow for the reported sensor data to violate the assumption

that the sensor’s vertical inertial reference is antiparallel with gravity. The propagation of

this error through the upper limb kinematics can manifest as an apparent tilting of the

calibration points plane away from horizontal. For this reason, we’ll also consider the planar

constraint as optional with respect to the quality of optimization result with this constraint.

The final selection of constraint set is presented in Table 4-3 along with indication

of the necessity for each constraint.

Table 4-3: Calibration constraint set

Constraint Necessity

Bounds Necessary

Distance Necessary

Orthogonality Necessary

Planar Optional (horizontal calibration points plane)

Normal Vector Vertical Inequality Necessary

4.4.11 Solving the Calibration Problem

The general problem we set out to solve is given by the objective function and

constraints developed in the preceding content of this section. A formal statement of the

problem is given here in (4-55).

112

minimize

𝒙𝑓(𝒙)

subject to 𝒈𝐵 ≼ 𝟎 𝑔𝑉 ≤ 0 ℎ𝐷21 = 0

ℎ𝐷23 = 0

ℎ𝐷31 = 0

ℎ𝑂 = 0

(4-55)

The objective function 𝑓(𝒙) is given by (4-33) and the locations of the constraint

formulas are given in Table 4-2.

The calibration optimization problem of (4-55) is quadratic with constant

(bounded), linear, and quadratic constraints. Although convexity is a desirable property of

an optimization problem such as this, we have at least one concave quadratic constraint in

𝑔𝑉 since 𝑸𝑉 is not positive semi definite. This matrix has both positive and negative

eigenvalues, and so the constraint function cannot be convex. The most suitable built in

solver in MATLAB is then the general nonlinear solver fmincon(). In the remainder of this

section, we discuss the process of solving the optimization problem in MATLAB using this

solver.

We must supply fmincon with an initial value for the design vector 𝒙0. This value

is computed by default by using rough measurements of a normative subject’s upper arm

dimensions along with an assumption on relative pose between the subject and the

calibration jig. The former specifies initial values for the upper limb lengths directly while

the latter is used to compute the calibration point locations using the known geometry of the

calibration jig. In a production application of this system, it should be reasonable to use

113

default values for any subject, but fall back on prompting the subject to manually enter

measured values if the calibration fails because of a poor initial condition.

Other required inputs to fmincon() include the implementation of the set of

constraints. This includes direct provision of the bounds on 𝒙 as well as 𝑨𝑃 and 𝒃𝑃 to specify

the linear constraints in 𝒉𝑃 in addition to functions that compute the objective function and

the nonlinear constraints.

Optional configuration of the solver that has been implemented includes selection

of the feature that uses analytical gradients from the constraints and the objective function.

Since we’ve implemented this feature in both the objective function and the constraint

function implementations, we can specify this option in a call to optimoptions() as

shown here.

optimOpts = optimoptions('fmincon',...

'gradobj','on',...

'gradconstr','on');

[...] = fmincon(...,optimOpts);

Additional options that can be set for fmincon() include such settings as

tolerances on the objective function, design vector, or constraints, specification of various

built in solver algorithms, and more. Since this problem is very well behaved, in particular

with satisfactory choice of 𝒙0, the solvers for each available algorithm successfully find the

same local optimum with an accuracy on the order of about two millimeters. This level of

apparent accuracy is likely more precise than the compounding of errors that are evident in

the physical nature of the problem.

114

4.5 Experiment Definition

The first requirement for any experimental trial we implement in this system is

calibration. For each trial, the subject must begin by holding the t-pose so that the home pose

can be set. Then, a short time series of data must be captured from each of three calibration

points located on the calibration jig to enable the calibration of the task space pose and the

subject’s upper limb geometry. Then, some exercise should be performed on which the

forward and inverse kinematics mappings of the sensor data can be analyzed to provide

information about the motion of the subject’s upper limb.

In this work we choose the exercise for this experiment to be a reaching task for

various reasons. Furthermore, we choose to use the subject’s motion between calibration

points, during the calibration procedure, as the experimental reaching task. This allows us

the benefit of having the action data captured for each task at a minimal time offset from the

time at which the calibration data is collected. This affects the validity of each trial’s

calibration data since we assume that the sensors will experience drift over time. In addition

to reducing the time between calibration and action performance, we also incorporate new

calibration data with every action sequence without increasing the total time for collection

of the experimental data. This affords us the highest possible calibration fidelity within the

chosen calibration framework.

The formal statement of experimental protocol is defined by assigning a timed

progression of well-defined states for the subject to follow during each experimental trial.

This protocol is defined by four states, home, idle, calib, and reach. The home state is the

starting state for which the subject should assume the t-pose and begin the remaining

automated procedure. The next state, idle, is then interleaved between all remaining state

115

changes to allow a brief period of time in which the subject can prepare for the upcoming

state change. The calib state is then met for each of the three calibration points with the

subject performing a reach between them.

The calib state is used to collect sufficient calibration data. During this state the

subject should smoothly articulate the upper limb joints as much as comfortably possible

while holding Sphero reliably in the appropriate calibration point fixture. When moving

between the calibration points, the subject will be performing the reach state. A reach should

be performed smoothly at approximately constant speed so that the reach begins and ends

at the boundaries of the allotted time duration. The reaching motion should involve the

minimal amount of vertical motion that is sufficient for Sphero to clear the calibration point

fixture, while the horizontal motion should trace a straight line segment between the

calibration point locations. Table 4-4 summarizes the state progression and timing for this

experimental protocol.

Table 4-4: Experimental protocol state progression and timing

State Location Duration

home t-pose ---

idle --- 2s

calib 𝐶1 3s

idle --- 2s

reach 𝐶1 → 𝐶2 3s

idle --- 2s

calib 𝐶2 3s

idle --- 2s

116

State Location Duration


idle --- 2s

calib 𝐶3 3s

idle --- 2s


idle --- 2s

calib 𝐶2 3s

4.5.1 Motion Analysis

At the position level, we will begin by computing a position error vector at every

time instant for the motion between two calibration points 𝐶𝑖 and 𝐶𝑗 . We assume that the

ideal motion between these two points is such that point 𝑆 is in the vertical plane including

these two points. We proceed to resolve the known relative position of 𝑆 with respect to 𝐶𝑖

into components that are parallel (𝒓𝑃/𝐶𝑖) and perpendicular (𝒓𝑆/𝑃) to this plane as shown in

Figure 4-6. The latter is the plane error vector we wish to calculate.

Figure 4-6: Experimental analysis plane error vector calculation

We begin by writing the vector loop closure equation in (4-56).

117

𝒅𝑆𝐹 = 𝒅𝐶𝑖

𝐹 + 𝒓𝑆/𝐶𝑖

(4-56)

We wish to resolve 𝒓𝑆/𝐶𝑖 into it’s components that are parallel and perpendicular

to the plane and meet at the intermediate point 𝑃, which is the point on the plane that is

closest to 𝑆. Next, we write the unit vector 𝒏𝑖𝑗 that is normal to the plane.

𝒏𝑖𝑗 =𝒓𝐶𝑖/𝐶𝑗 × 𝒛𝑇

‖𝒓𝐶𝑖/𝐶𝑗 × 𝒛𝑇‖

(4-57)

We can calculate our desired position error vector 𝒆𝑝 = 𝒓𝑆/𝑃, the component of

𝒓𝑆/𝐶𝑖 perpendicular to the plane via projection onto 𝒏𝑖𝑗.

𝒆𝑝 = 𝒓𝑆/𝑃 = (𝒏𝑖𝑗

T 𝒓𝑆/𝐶𝑖)𝒏𝑖𝑗

(4-58)

The formula for 𝒆𝑝 in (4-58) is fully known by the relationships in (4-56) and

(4-57).

118

5 Motion Analysis

In this section we apply the work from all previous sections toward the

experiment implementation. First, we discuss the experimental setup including the physical

hardware required and application specific parameters used in calibration and analysis.

Then, in the data collection stage, we describe the software implementation that is used to

control Myo and Sphero at a high level. Finally, we post process and calibrate the data before

performing analysis in the way of empirical validation tests that are used to assess the

kinematic modeling and effectiveness of the motion capture system.

5.1 Experimental Setup

The experiments will take place in an office-like environment with the subject

seated at a desk. In this section, we’ll introduce the physical realization of the theoretical

calibration fixtures, calibration jig assembly, and the geometry of the subject within the

experimental task space given in terms of the mathematical model.

The proposed calibration method involves the use of a calibration jig in which

calibration point fixtures are installed at known relative locations. The calibration point

fixture shown in Figure 5-1 is designed to cup Sphero such that it can undergo spherical

rotations while the center point of Sphero remains fixed at the calibration point.

Figure 5-1: Calibration point fixture

119

These fixtures are designed with dimensions that are largely arbitrary except for

the radius that cups Sphero. The manufactured radius for this feature is sized roughly 0.5mm

larger than the radius of Sphero to minimize contact interference. This dimension detail is

shown along with the important assembly dimensions for the calibration jig in Figure 5-2.

Figure 5-2: Calibration fixtures assembled onto jig

Dimensions given in millimeters

The calibration jig is designed with its shape and size appropriate to be laid flat

on the top of a desk. The calibration points are defined as (top-left of Figure 5-2) 𝐶1: top-left,

𝐶2: bottom-left, and 𝐶3: bottom-right. This definition corresponds with the relative position

vectors 𝒓𝐶2/𝐶3 and 𝒓𝐶2/𝐶1 orthogonality requirement, and the required lengths of these

vectors are also provided in this assembly drawing.

The physical realization of the calibration jig has been manufactured from a 3/4in

thick piece of medium density fiberboard for the base, and three calibration fixtures that are

3D-printed from polylactic acid (PLA) filament. The fixtures are attached to the base with

120

four wood screws each as are four rubber feet (not pictured) to provide stability on a variety

of surfaces. The final product can be seen in Figure 5-3.

Figure 5-3: Experimental setup calibration jig and subject

Note that the subject is not wearing the Myo devices in this image.

The calibration jig and subject shown in Figure 5-3 allow us to envision the most

proximal and most distal coordinate frames in our upper limb motion model, the task frame

𝑇 and the fixed frame 𝐹, respectively. We recall that the calibration optimization is seeded

with initial values of the design vector for the calibration point locations 𝒅𝐶𝑖𝐹 . We can

compute that information by approximating the location of the origin of frame 𝐹 with

respect to the calibration point 𝐶3 . The two required measurements are indicated by the

dashed grey line segments assuming that the subject is consistently positioned so that the

origin of 𝐹 is aligned with 𝐶3 in the 𝒙𝐹 direction. Assuming that the relative rotation between

𝐹 and 𝑇 is roughly identity, then 𝒅𝑇𝐹 is calculated directly from the approximated dimensions

and our knowledge of the calibration jig geometry. In these experiments the initial value

121

𝒅𝑇𝐹 = [300 −250 −250]Tmm is used. From this information, we can then compute the

initial values for 𝒅𝐶𝑖𝐹 .

5.2 Data Collection

The data collection procedure involves the user operating a purpose-built GUI to

operate a custom class implementation that manages control of the Myo devices, the Sphero

device, and the data. The GUI allows users to connect the devices, manually toggle the

locations of the two Myos (upper and lower limb segments), set the home pose, and save the

current data set all while visualizing the streaming data in a dynamically updating figure.

The device management class MyoSpheroUpperLimb owns references to

MyoMex and Sphero objects (stored as class properties) that provide the interface to the

two Myos and Sphero that are used in this implementation. A method called

connectDevices() performs the instantiation of the device class properties while also

capturing the delay between these operations to synchronize the data sources when

accessing the data.

The joint rotation data log is accessed by removing the most recent samples from

the internally managed queue with the method popRotData() to return a structure

containing the uncalibrated sensor data for 𝑹𝑈𝐹 𝑹𝐿

𝐹 and 𝑹𝐻𝐹 . This sensor rotation data is

internally accessed through dependent properties RX_source (X is U, L, or H) that return

data that has been calibrated to a home pose. These accessors check the myoOneIsUpper

property to appropriately assign data from the two Myo devices to the correct data sources,

and then remove the home pose orientation offset that is stored in properties named RFNX.

122

Another utility method setHomePose() is also implemented to automates the setting of

RFNX properties to the current value of RX_source(:,:,end).

Visualization of the home calibrated data is provided by internally managed

named plots of the subject’s upper limb as shown in Figure 5-4. This plot is created and

updated by calling a method drawPlotUpperLimb() with a unique name string,

calibration data struct, and action data struct containing only one sample of data to visualize.

Animation of a data sequence, or the monitoring of data in near real time, can be performed

by scheduling calls to this method with a timer object.

Figure 5-4: Data visualization provided by MyoSpheroUpperLimb

Visualization provided by drawPlotUpperLimb() with the addition of desired and actual trace of Sphero position

The visualization depicts the current pose of the limb segments computed based

on the internally stored lengths of the limb segments 𝒍 as well the calibration jig geometry

positioned at internally stored values for 𝒅𝑇𝐹 and 𝑹𝑇

𝐹 . Upon instantiation these values are set

at default parameters discussed earlier and given by the nominal anatomical dimensions

123

assumed in the computer generated graphical model of the upper limb segments. As these

values are updated, the plot will also dynamically update to show the changes.

When the data collection GUI is launched the user first uses a pushbutton to

instantiate MyoSpheroUpperLimb. Once connection is established to the devices, the user

should then click a pushbutton to invoke the visualization tool while using a pushbutton to

set the home pose to the currently performed t-pose (shown in Figure 5-5) and a checkbox

to correctly configure myoOneIsUpper property.

Figure 5-5: Subject performing the t-pose to set the home pose

Each time setHomePose() pushbutton is clicked a stopwatch style timer is

displayed on the GUI and popRotData() discards the data log to flush the log queues. This

is intended to be used by the user and the subject to properly time the subject’s motions

according to the experimental protocol. Then, when the subject has correctly performed the

experimental protocol for one trial, a pushbutton is used to save the calibration and action

data sequence in a timestamped .mat file named trial-yyyymmdd-HHMMSS.mat.

124

5.3 Data Processing and Calibration

After the collection of data for an experimental trial, the data must be segmented

according to the protocol states defined in Table 4-4 so that calibration and analysis can be

performed. In the best case implementation scenario this would be performed automatically

upon successful saving of trial data in near real time. Although this is possible to implement

with the current system, it has proven to be unreliable since the resource demands of

SpheroCore are too high while streaming data. The remainder of data processing and

calibration is performed offline after the data collection phase is complete.

The time durations for the states (Table 4-4) along with the known sample rate of

the data are used to segment the complete data log by into two separate sets of three logs

each. The three calib state data sets and the three reach state data sequences are each stored

in a three element struct array for further processing. The calib state data set struct, named

calibPointsData, is then passed on to a collection of methods in

MyoSpheroUpperLimb to perform the calibration.

The invocation of MyoSpheroUpperLimb.calibrate(calibPointsData)

sets off the procedure to perform calibration optimization on the calibration points data set.

The body of this method is shown below to illustrate the way in which the calibrate()

method configures and invokes the calibration optimization in its call to the static method

computeCalibration(). After the calibration has been performed, the calibration

results are then interpreted to by computing 𝒍 𝒅𝑇𝐹 and 𝑹𝑇

𝐹 from the optimization solution 𝒙∗.

function calibrate(this,calibPointsData)

calib = this.calib;

data = calibPointsData;

params = this.makeCalibParams(...

calib,data,{'ortho','dist','normalVertIneq'});

[xs,fval,exitFlag,output,lambda] = this.computeCalibration(params);

125

[lengths,dT,RT] = this.interpretCalibResult(xs);

this.lengthUpper = lengths(1);

this.lengthLower = lengths(2);

this.lengthHand = lengths(3);

this.dT = dT;

this.RT = RT;

end

The calibration parameters struct params contains information that is critical to

the optimization such as an initial guess of the design vector, encoding of the objective

function parameters, and an encoding of the selected constraints. The latter is specified by

the cell array of strings params.conSpec where constraints are selected by indicating the

names from Table 4-2.

function [xs,fval,exitFlag,output,lambda] = computeCalibration(params)

LB = [0;0;0;-inf(9,1)];

UB = inf(12,1);

Aeq = []; beq = [];

[Aeq_,beq_] = MyoSpheroUpperLimb.planarFun(params);

Aeq = [Aeq;Aeq_]; beq = [beq;beq_];

A = []; b = [];

x0 = [...

params.lengthUpper;...

params.lengthLower;...

params.lengthHand;...

params.dT0 - params.rc2c1;...

params.dT0;...

params.dT0 - params.rc2c3];

optimOpts = optimoptions('fmincon',...

'display','iter',...

'gradobj','on',...

'gradconstr','on');

objFunGrad = @(x)MyoSpheroUpperLimb.objFun(x,params);

nonlinConFunGrad = @(x)MyoSpheroUpperLimb.nonlinConFunGrad(x,params);

[xs,fval,exitFlag,output,lambda] = ...

fmincon(objFunGrad,x0,...

A,b,Aeq,beq,LB,UB,...

nonlinConFunGrad,...

optimOpts);

if exitFlag<1

warning('Calibration may not be successful!');

end

end

Ten trials were captured in total from which one suffered data loss as a result of

user error, and one suffered incorrect calibration result due to the subject’s invalid

126

performance of the experimental protocol. The remaining eight trials were then arbitrarily

reduced to five representative trials for the reporting of results here. Table 5-1 shows the

calibration optimization statistics.

Table 5-1: Calibration optimization statistics Brief statistics for the calibration optimization step using the Sequential Quadratic Program algorithm

Trial Iterations Objective Function Value

First Order Optimality

Constraint Violation

1 19 624005.1446 0.001830164 3.78786E-08

2 23 539046.6033 0.000212945 9.54606E-09

3 20 533839.1261 0.002044092 4.64497E-08

4 24 554550.4986 0.003572365 5.12227E-08

5 20 567094.4731 0.001807924 7.14499E-09

Each optimization solved relatively quickly with only about twenty iterations. The

solver stopped in each case because the change in the objective function became less than its

tolerance with no active constraints. The constraint violation is also negligible since its units

are at most on the order of squared millimeters. Practically, any constraint violation up to

the order of a whole millimeter is reasonably acceptable for the accuracy of the kinematic

model and the rotation data in this work.

We can also look at the resulting optimal design vector to gain insight on the

effectiveness of this approach. Table 5-2 shows the resulting geometric parameters of the

subject’s upper limb. These values should theoretically be constant between trials, and

excessive variations here may indicate errors in the model and data.

127

Table 5-2: Calibration subject geometric parameter results

Trial Upper 𝒍𝑼 [mm]

Lower 𝒍𝑳 [mm]

Hand 𝒍𝑯 [mm]

Total 𝒍𝑼 + 𝒍𝑳 + 𝒍𝑯 [mm]

1 300 239 87 627

2 319 228 55 603

3 347 206 70 623

4 325 216 78 619

5 317 218 79 615

minimum mean maximum range

300 322 347 47

206 221 239 33

55 74 87 32

603 617 627 24

These limb segment length values are qualitatively correct. The values decrease

in magnitude from upper to lower to hand. The values are also near the rough values

expected from approximating these dimensions by measuring them with a ruler. However,

the variation of the segment lengths over multiple trials is more interesting. If we compare

the range of values by normalizing them to the mean of values, we see similar results for

upper and lower limb segments at 15%, but much greater variation in the hand at 44%. The

total length of all segments varies much less with a mean normalized range of 4%. This may

suggest that the model is not failing at capturing the subject’s geometry, but rather failing at

representing it properly. However, the absolute ranges of this length data are all roughly the

same with variation from 24mm to 47mm so that this interpretation of results may be

merely a consequence of the true magnitude for each value. It may be possible that the

experimental protocol does not allow for the subject to articulate the upper limb in the best

way to activate the range of motion required in the joints to properly calibrate the upper

limb geometry.

128

5.4 Analysis Results

The analysis is performed on the three reach tasks contained in each of these five

trials. For each task, we consider three evaluation metrics. First, we compute the magnitude

of the plane error 𝑒𝑝 = ‖𝒆𝑝‖ of (4-58) to provide an indication of how well the subject

performed the intended motion. Then, we qualitatively and quantitatively investigate the

results from the inverse kinematics mapping developed in section 4.3.2. The latter is

considered to be the distance error in 𝒅𝑆𝐹 before and after the inverse kinematic mapping

modifies the motion data.

In this section, we choose to show the analysis results for a representative trial

number 4. A visualization of the calibration data is shown in Figure 5-6 for qualitative

inspection. We see that the forward kinematics result does not exactly correspond with the

locations of the calibration points for all three calibration points (top row). Since this will

have an effect on the relationship between a perfect plane motion in the data representation

compared to the assumed perfect plane motion, it will likely have an effect on the results of

the plane error magnitude 𝑒𝑝.

129

Figure 5-6: Calibration data visualization for trial 4

Visualization of calibrated data (top) and experiment video (bottom) at each calibration point

The compounded effect we expect this calibration correspondence error to have

on 𝑒𝑝 is illustrated in Figure 5-7. A translation of the perfect motion plane (left) in the data

will result in a constant offset of the true 𝑒𝑝, and a rotation will result in a v-shaped offset of

𝑒𝑝 with the minimum representing the intersection of the two planes. These plots are purely

geometrical in that the time evolving trajectory of the point of interest is not represented.

Some warping of these characteristic curve shapes due to acceleration is likely to be evident

in offset of the real data.

Figure 5-7: The effect of poor calibration correspondence on plane error

130

Figure 5-8 shows the plane error plotted against sample number for all three

reach tasks of trial 4. We see that this error is bounded within 8cm and the majority of the

data is bounded by 5cm . We also see the predicted characteristic shapes of Figure 5-7

evident in the data for 𝐶1 → 𝐶2 and 𝐶3 → 𝐶1 while the error for 𝐶2 → 𝐶3 is much higher and

in a shape that is inconsistent with the expected sources of error other than human error

itself that we intended to capture.

Figure 5-8: Magnitude of plane error ep for three reaches in trial 4

The joint angle trajectories for the reach 𝐶2 → 𝐶3 are shown in Figure 5-9. As could

be expected by intuition we can check that the majority of the motion contribution comes

from 𝜃𝑠𝑧 𝜃𝑠𝑦 and 𝜃𝑒𝑧. We also note that the assumption in (4-27) appears to be approximately

satisfied with a value very near to 0°.

131

Figure 5-9: Inverse kinematics joint angle trajectories

The validity of the inverse kinematics data is then verified by calculating 𝒅𝑆𝐹 by

using the sensor data directly as well as the result from inverse kinematics calculation of the

joint angles. The distance between these two calculations for 𝒅𝑆𝐹 is shown in Figure 5-10 for

all three reaches. The maximum error here is bounded at 3.5cm which is significantly less

than the plane error bounds resulting largely from calibration errors. Sources of this error

are possibly due to either sensor inaccuracies such as drift or mechanical errors such as the

relative motion between the sensor and the subject’s arm due to skin movement.

132

Figure 5-10: Magnitude of error introduced by inverse kinematics

133

6 Discussion

This exercise in applying COTS devices to human motion analysis has been

majorly successful as the first iteration in this motion capture system’s engineering

development. The software tools developed to communicate with Myo and Sphero from the

MATLAB environment were successful at serving their intended feature sets, although

certain limitations have affected their usability and accuracy of results. The calibration

procedure and method has been shown to function correctly with the small set of trials

tested in this work. However, the accuracy of the calibration leaves much room for

improvement. And the method of human upper limb kinematic modeling is also shown to be

correct although it may introduce unnecessary errors.

The Myo SDK MATLAB MEX Wrapper project has not presented any problems

even after substantial use in this project. Furthermore, at the time of this writing, the

community on The MathWorks’ File Exchange has provided a strong endorsement in the way

of twenty-one 5 star ratings and a monthly download average between forty and sixty

downloads per month for a duration of ten months [14]. This project has been used by

everyone from software developers to undergraduate students and graduate researchers to

engineering faculty and professionals. A variety of applications have been reported including

use in robot controllers, medical research, and this package has even been used to win a

hackathon.

Although this Myo package has brought such overwhelming success to the greater

academic and development communities as well as this work, the feature limitations with

EMG streaming are an impediment to future progress that should be addressed. Many

applications, with motor rehabilitation included, would find great benefit if the MATLAB

134

interface to Myo would support streaming EMG data from more than one sensor

simultaneously. This limitation can be removed in future work by the replacement of the

Myo Connect desktop application with a new software solution that provides data from

multiple Myos that are paired to different Bluetooth dongles.

Sphero API MATLAB SDK was also a great success, although it has not raised quite

the same popularity as the Myo project. Since its initial release in the summer of 2015, this

project has been downloaded roughly ten to fifteen times per month, and it also maintains a

five star rating [15]. User applications that are built on this code have been developed by

undergraduate students, professional researchers, and even nontechnical graduate

researchers. The projects undertaken by these groups include computer vision feedback

controlled choreography of Sphero, oceanographic data collection (leveraging Sphero’s

waterproof quality), and rat evader experiments to understand the trajectory planning

algorithms used by rats.

The success of projects that interact with a single Sphero device and come with

very little computational cost otherwise perform quite well with the current MATLAB

interface software for Sphero. However, when adding resource demands to the MATLAB

session in which Sphero is being operated, a certain limiting threshold exists, beyond which

the functionality of the Sphero interface breaks down. This is viewed as a performance

limitation that may very well have affected the results in this work. The manifestation of this

deficiency is evident typically when streaming data from Sphero at a sufficiently high data

rate, and is identified by lag in the processing of scheduled tasks. For instance, users will

notice that an animation plot update scheduled by a timer will lag behind the expected

update time during symptomatic scenarios. This is due to the inability of MATLAB to quickly

135

process the callbacks generated by the built in Bluetooth object when new incoming bytes

become available. The compounding nature of computational overhead between MATLAB

m-code and the Bluetooth data through many abstraction layers leads to this performance

limitation. The many layers include the MATLAB compiler runtime environment, the Java

virtual machine MATLAB runs within, the operating system, and the Bluetooth hardware

itself. Future work can address this limitation by providing the Bluetooth implementation in

a MEX file relying upon a third party Bluetooth library instead of the m-code implementation

that uses the MATLAB Bluetooth object. In a fashion similar to that with which Myo SDK

MATLAB MEX Wrapper has been developed, the m-code interface can then be built on top of

this lower level base.

The collective utility of the software tools for Myo and Sphero has been shown in

this work by the application cases presented in section 3.3 for the individual software tools

as well as in the main application to human upper limb motion analysis that is the focus of

this work. A measurable intended outcome of software development is seen in the code size

for the CLI scripts in 3.3.1 and 3.3.3 for Myo and Sphero, respectively. When we compare the

MATLAB m-code size for a simple data collection exercise to the total code size for each of

these projects, we see a great reduction. For Myo, we compare a 5 line script to a total of

roughly 1500 source lines of code in the implementation of Myo SDK MATLAB MEX Wrapper.

In a similar way, we see a reduction to a 9 line script from a MATLAB implementation that’s

almost 3000 source lines of code. This massive reduction in the size of the code required to

perform simple to moderately complicated tasks with Myo and Sphero in MATLAB is a factor

that contributes to the accessibility of these devices to a larger population of students and

researchers in academia. In addition to reduction in code size, we also see a great lessening

136

of the specific programming language knowledge required to use these devices in MATLAB.

Much of the strict syntax and logic encoded in these projects has been abstracted away from

the end user’s experience so that even nontechnical researchers or technical students who

lack previous programming experience may be enabled to use these devices.

The calibration errors discovered in section 5.4 are suspected to be caused by

many contributing sources including excessive calibration jig manufacturing tolerances, data

synchronization errors, and the violation of kinematic modeling assumptions. The latter two

are more likely than the former to have been the dominant sources for error in this work.

Data synchronization error may be related to the same cause as the Sphero

interface performance limitation described previously. When the Bluetooth object event

queue is not processed as quickly as it is generated, the most recent data available to the user

will become progressively older as the lag increases. The apparent quality of data

synchronization based upon the lagging Sphero data will influence the users to assign

misguided trust in the synchronization quality between the Sphero data source and the Myo

data sources. With the multiple data sources out of synchronization, the recorded data for a

given time instance will be invalid with respect to the physical phenomenon that’s intended

to have been captured. This is a problem that, if existent during calibration, will affect the

accuracy of all future calibrated data elements.

The kinematic modeling assumption that is perhaps most susceptible to violation

is that the origin of the shoulder joint remain fixed in space. This assumption requires the

subject to be responsible for the introduction of error directly into the data set if the shoulder

is moved. A translation of the shoulder maps into a translation of any distal point on the

upper limb by the same amount. It’s quite possible that a substantial portion of the observed

137

error in this work is due to unintentional motions by the subject in violation of this

assumption. Future work should use complementary technologies to observe translational

information about the upper limb kinematics in addition to the rotational position

information currently used from the IMU sources. Incorporation of the data from a vision

based sensor such as the Microsoft Kinect may all of the benefits of vision based and contact

based sensors in one integrated motion capture system.

The present work has proposed a motion capture system for the human upper

limb based in the use of the COTS devices Myo and Sphero. The necessary interface software

has been developed for use of these devices by researchers with minimal programming

experience to interact with the best quality data from these devices in near real time. An

upper limb kinematic model, calibration procedure, and experimental protocol were

developed as a framework within which to test the function and performance of the interface

software as well as the modeling methodology itself. The calibration procedure includes the

determination of unknown geometry of the subject as well as the virtual configuration of the

subject’s task environment. This knowledge of the task environment enables the use of both

physical and virtual fixtures in motion analysis application such as motor rehabilitation

therapy task evaluation.

The present work has provided a major contribution to researchers across the

globe by providing the first fully featured and fully functional MATLAB m-code interface to

Thalmic Labs’ Myo armband [14]. The software tools for Sphero were also the first of their

kind to have been released publicly as a device interface for MATLAB [15]. In contrast to the

typical choice of Microsoft Kinect as a COTS device for human motion analysis, the present

work has taken a different approach to motion capture utilizing low quality consumer grade

138

contact based IMU sensing technology. Considering the challenges associated with end-to-

end development of such a motion capture system, the display of reasonably correct results

to calibration and motion analysis proves to be a third major contribution of this work.

Finally, the set of recommendations for future work in all aspects of this project sets forth a

path to follow into the next iteration of human motion capture system design.

139

Appendix A Source Code

Here we collect the textual content of source code files that are not prohibitively

long (greater than 1000 source lines of code), and that aid the discussion in the main body

text. Note that all source code written for this project can be found at the author’s GitHub

(https://github.com/mark-toma/) in the public repositories MyoMex and SpheroMATLAB

[16], [18].

Appendix A.1 myo_class.hpp

#ifndef MYO_CLASS_HPP

#define MYO_CLASS_HPP

// comment the following line to remove debug output via DB_MYO_CLASS()

//#define DEBUG_MYO_CLASS

#ifdef DEBUG_MYO_CLASS

#define DB_MYO_CLASS(fmt, ...) printf(fmt, ##__VA_ARGS__)

#else

#define DB_MYO_CLASS(fmt, ...)

#endif

#include "mex.h"

#include "myo/myo.hpp" // myo sdk cpp binding

#include <array> // myo sdk emg data

#include <vector>

#include <queue>

// --- Data Frames

// These structures are used as return types in MyoData and DataCollector

// methods to return a single sample of data from one time instance

// IMU data frame

struct FrameIMU

{

myo::Quaternion<float> quat;

myo::Vector3<float> gyro;

myo::Vector3<float> accel;

myo::Pose pose;

myo::Arm arm;

myo::XDirection xDir;

}; // FrameIMU

// EMG data frame

struct FrameEMG

{

std::array<int8_t,8> emg;

}; // FrameEMG

https://github.com/mark-toma/

https://github.com/mark-toma/MyoMex

https://github.com/mark-toma/SpheroMATLAB

140

// END Data Frames

// --- MyoData

// This class is used to keep track of the data for one physical Myo.

// Typical use may be to instantiate a MyoData for each unique myo received

// by a DataCollector. Then, subsequent calls into add<data> and get<data>

// can be used to receive contiguous collections of IMU, EMG, and Meta data

class MyoData

{

// Data frames for returning data samples

FrameIMU frameIMU;

FrameEMG frameEMG;

// Pointer to a Myo device instance provided by hub

myo::Myo* pMyo;

bool addEmgEnabled;

// IMU data queues and state information

std::queue<myo::Quaternion<float>,std::deque<myo::Quaternion<float>>> quat;

std::queue<myo::Vector3<float>,std::deque<myo::Vector3<float>>> gyro;

std::queue<myo::Vector3<float>,std::deque<myo::Vector3<float>>> accel;

std::queue<myo::Pose,std::deque<myo::Pose>> pose;

std::queue<myo::Arm,std::deque<myo::Arm>> arm;

std::queue<myo::XDirection,std::deque<myo::XDirection>> xDir;

uint64_t timestampIMU;

unsigned int countIMU;

// EMG data queues and state information

std::queue<std::array<int8_t,8>,std::deque<std::array<int8_t,8>>> emg;

unsigned int semEMG;

unsigned int countEMG;

uint64_t timestampEMG;

// syncIMU

// Called before pushing quat, gyro, or accel

// This updates the timestampIMU member to keep track of the IMU datas.

// If it is detected that a sample of quat, gyro, or accel was skipped,

// the previous value for that data source is copied to fill the gap.

// This is zero-order-hold interpolation of missing timeseries data.

// Furthermore, since event-based meta data is sampled on the EMG vector

// as well, we fill their queues up to the future size of emg to maintain

// consistency there.

void syncIMU(uint64_t ts)

{

if ( ts > timestampIMU ) {

// fill IMU data (only if we missed samples)

while ( quat.size() < countIMU ) {

myo::Quaternion<float> q = quat.back();

quat.push(q);

}

while ( gyro.size() < countIMU ) {

myo::Vector3<float> g = gyro.back();

gyro.push(g);

141

}

while ( accel.size() < countIMU ) {

myo::Vector3<float> a = accel.back();

accel.push(a);

}

countIMU++;

timestampIMU = ts;

// fill pose, arm, and xDir up to the new countEMG

myo::Pose p = pose.back();

while ( pose.size()<(countIMU-1) ) { pose.push(p); }

myo::Arm a = arm.back();

while ( arm.size()<(countIMU-1) ) { arm.push(a); }

myo::XDirection x = xDir.back();

while ( xDir.size()<(countIMU-1) ) { xDir.push(x); }

}

}

// sync<Pose/Arm/XDir>

// This event-based meta data is sampled on the EMG vector, so we fill

// their queues up to the future size of emg to maintain consistency.

// Things would theoretically break down if these events fired more

// frequently than the emg data, but I think that's impossible. It's

// highly unlikely for sure! But still, the boolean return values allow

// for guarding against this case.

bool syncPose(uint64_t ts)

{coll

if (pose.size() == countIMU)

return false;

myo::Pose p = pose.back();

while ( pose.size()<(countIMU-1) ) { pose.push(p); }

return true;

}

bool syncArm(uint64_t ts)

{

if (arm.size() == countIMU)

return false;

myo::Arm a = arm.back();

while ( arm.size()<(countIMU-1) ) { arm.push(a); }

return true;

}

bool syncXDir(uint64_t ts)

{

if (xDir.size() == countIMU)

return false;

myo::XDirection x = xDir.back();

while ( xDir.size()<(countIMU-1) ) { xDir.push(x); }

return true;

}

// syncEMG

// Called before pushing emg data

// This updates the timestampEMG member to keep track of the EMG datas.

// If it is detected that a sample of emg was skipped, the previous value

// for that data source is copied to fill the gap. This operation sounds

// trivial, but it isn't quite as simple as you'd expect. Myo SDK

// provides emg data samples in pairs for each unique timestamp. That is,

// timeN emgN

142

// time0 emg1

// time0 emg2

// time1 emg3

// time1 emg4

// timeK emg(2*K+1)

// timeK emg(2*K+2)

// So then, we keep track of the number of new emg samples received

// without a new timestamp in semEMG. Then pad emg with the last value if

// it's detected that a sample was missed.

// This is zero-order-hold interpolation of missing timeseries data.

void syncEMG(uint64_t ts)

{

if ( ts>timestampEMG ) { // new timestamp

if ( 0==(semEMG%2) ) {

std::array<int8_t,8> e = emg.back();

emg.push(e);

}

semEMG = 0; // reset sem

} else {

semEMG++; // increment sem

}

countEMG++;

timestampEMG = ts;

}

public:

MyoData(myo::Myo* myo, uint64_t timestamp, bool _addEmgEnabled)

: countIMU(1), countEMG(1), semEMG(0), timestampIMU(0), timestampEMG(0)

{

pMyo = myo; // pointer to myo::Myo

// perform some operations on myo to set it up before subsequent use

pMyo->unlock(myo::Myo::unlockHold);

if (_addEmgEnabled) {

pMyo->setStreamEmg(myo::Myo::streamEmgEnabled);

countEMG = 1;

std::array<int8_t,8> _emg;

emg.push(_emg);

timestampEMG = timestamp;

}

addEmgEnabled = _addEmgEnabled;

// fill up the other private members

myo::Quaternion<float> _quat; // dummy default objects

myo::Vector3<float> _gyro;

myo::Vector3<float> _accel;

quat.push(_quat); // push them back onto queues

gyro.push(_gyro);

accel.push(_accel);

pose.push(myo::Pose::unknown);

arm.push(myo::armUnknown);

xDir.push(myo::xDirectionUnknown);

timestampIMU = timestamp;

}

// Myo is owned by hub... no cleanup necessary here

143

~MyoData() {}

// getFrameXXX

// Read a sample of data from the IMU or EMG queues

FrameIMU &getFrameIMU()

{

countIMU = countIMU - 1;

frameIMU.quat = quat.front();

frameIMU.gyro = gyro.front();

frameIMU.accel = accel.front();

frameIMU.pose = pose.front();

frameIMU.arm = arm.front();

frameIMU.xDir = xDir.front();

quat.pop();

gyro.pop();

accel.pop();

pose.pop();

arm.pop();

xDir.pop();

return frameIMU;

}

FrameEMG &getFrameEMG()

{

countEMG = countEMG - 1;

frameEMG.emg = emg.front();

emg.pop();

return frameEMG;

}

// getInstance

// Get the pointer to this myo::Myo* object. Use this function to test

// equivalence of this MyoData's myo pointer to another.

myo::Myo* getInstance() { return pMyo; }

// getCountXXX

// Get the number of valid samples in the IMU or EMG queues

unsigned int getCountIMU() { return countIMU; }

unsigned int getCountEMG() { return countEMG; }

// syncDataSources

// Pops data off of queues until there are at most two bad samples.

// Subsequently, a third bad sample may fill the read head of the queue.

// Use this functions to put the data vector into a known state. Throw

// away the first three samples of data read after this call. The rest

// should be contiguous on the maximum sample rate for the data source.

void syncDataSources()

{

FrameIMU frameIMU;

while ( getCountIMU() > 1 )

frameIMU = getFrameIMU();

FrameEMG frameEMG;

while ( getCountEMG() > 1 )

frameEMG = getFrameEMG();

}

// add<data> functions

// All of these perform two operations:

144

// * sync<type>

// Syncs up the data queues that are being samples on the same time

// base.

// * <data>.push(_<data>) pushes new data onto its queue

void addQuat(const myo::Quaternion<float>& _quat, uint64_t timestamp)

{

syncIMU(timestamp);

quat.push(_quat);

}

void addGyro(const myo::Vector3<float>& _gyro, uint64_t timestamp)

{

syncIMU(timestamp);

gyro.push(_gyro);

}

void addAccel(const myo::Vector3<float>& _accel, uint64_t timestamp)

{

syncIMU(timestamp);

accel.push(_accel);

}

void addEmg(const int8_t *_emg, uint64_t timestamp)

{

if (!addEmgEnabled ) { return; }

syncEMG(timestamp);

std::array<int8_t,8> tmp;

int ii = 0;

for (ii;ii<8;ii++) {tmp[ii]=_emg[ii];}

emg.push(tmp);

}

void addPose(myo::Pose _pose, uint64_t timestamp)

{

if ( syncPose(timestamp) )

pose.push(_pose);

}

void addArm(myo::Arm _arm, uint64_t timestamp)

{

if ( syncArm(timestamp) )

arm.push(_arm);

}

void addXDir(myo::XDirection _xDir, uint64_t timestamp)

{

if ( syncXDir(timestamp) )

xDir.push(_xDir);

}

}; // MyoData

// END MyoData

// --- DataCollector

// This class provides the link to Myo SDK, encapsulation of the MyoData

// class that manages data queues for each Myo device, and provides access

// to that data.

// * Register this class with a myo::Hub to trigger calls back into the

// on<event> functions below.

// * Call myo::Hub::run to allow callbacks to write data into the

// encapsulated MyoData objects in knownMyos

// * Call getFrameXXX(id) at most getCountXXX(id) times to read samples of

145

// FrameXXX data, where id is the 1-indexed id for a Myo device with

// maximum value getCountMyos()

class DataCollector : public myo::DeviceListener

{

std::vector<MyoData*> knownMyos;

public:

bool addDataEnabled; // unset to disable callbacks (they'll fall-through)

bool addEmgEnabled;

DataCollector()

: addDataEnabled(false), addEmgEnabled(false)

{}

~DataCollector()

{

// destruct all MyoData* in knownMyos

int ii=0;

for (ii;ii<knownMyos.size();ii++)

{

delete knownMyos[ii];

}

}

// --- Wrappers for MyoData members

// These functions basically vectorize similarly named members of MyoData

// on the elements of knownMyos

unsigned int getCountIMU(int id) { return knownMyos[id-1]->getCountIMU(); }

unsigned int getCountEMG(int id) { return knownMyos[id-1]->getCountEMG(); }

const FrameIMU &getFrameIMU( int id ) { return knownMyos[id-1]-

>getFrameIMU(); }

const FrameEMG &getFrameEMG( int id ) { return knownMyos[id-1]-

>getFrameEMG(); }

void syncDataSources()

{

int ii = 0;

for (ii;ii<knownMyos.size();ii++)

knownMyos[ii]->syncDataSources();

}

// getCountMyos

// Get current number of myos

const unsigned int getCountMyos() { return knownMyos.size(); }

// getMyoID

// Returns the (1-indexed) ID of input myo in knownMyos. If myo isn't in

// knownMyos yet, it's added. This function can be used to index into

// knownMyos with a myo pointer by: knownMyos[getMyoID(myo)-1].

const unsigned int getMyoID(myo::Myo* myo,uint64_t timestamp)

{

// search myos in knownMyos for myo

for (size_t ii = 0; ii < knownMyos.size(); ii++)

if (knownMyos[ii]->getInstance() == myo) { return ii+1; }

// add myo to a new MyoData* in knowmMyos if it doesn't exist yet

knownMyos.push_back(new MyoData(myo,timestamp,addEmgEnabled));

return knownMyos.size();

}

// on<event> Callbacks

146

// * Refer to the Myo SDK documentation for information on the mechanisms

// that trigger these callback functions in myo::Hub.

// * All of these invoke getMyoID() so as to automatically add myo to

// knownMyos without explicitly expressing this logic.

// * The on<data>Data functions fall-through when !addDataEnabled

// * Some device state meta data is maintained in the state change events

void onPair(myo::Myo* myo, uint64_t timestamp, myo::FirmwareVersion

firmwareVersion)

{

unsigned int tmp = getMyoID(myo,timestamp);

}

void onConnect(myo::Myo *myo, uint64_t timestamp, myo::FirmwareVersion

firmwareVersion)

{

unsigned int tmp = getMyoID(myo,timestamp);

}

void onDisconnect(myo::Myo* myo, uint64_t timestamp)

{

knownMyos.erase(knownMyos.begin()+getMyoID(myo,timestamp)-1);

}

void onLock(myo::Myo* myo, uint64_t timestamp)

{

// shamelessly unlock the device

myo->unlock(myo::Myo::unlockHold);

}

void onOrientationData(myo::Myo* myo, uint64_t timestamp, const

myo::Quaternion<float>& q)

{


knownMyos[getMyoID(myo,timestamp)-1]->addQuat(q,timestamp);

}

void onGyroscopeData (myo::Myo* myo, uint64_t timestamp, const

myo::Vector3<float>& g)

{


knownMyos[getMyoID(myo,timestamp)-1]->addGyro(g,timestamp);

}

void onAccelerometerData (myo::Myo* myo, uint64_t timestamp, const

myo::Vector3<float>& a)

{


knownMyos[getMyoID(myo,timestamp)-1]->addAccel(a,timestamp);

}

void onEmgData(myo::Myo* myo, uint64_t timestamp, const int8_t *e)

{

if (!addDataEnabled||!addEmgEnabled) { return; }

knownMyos[getMyoID(myo,timestamp)-1]->addEmg(e,timestamp);

}

void onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose p)

{


knownMyos[getMyoID(myo,timestamp)-1]->addPose(p,timestamp);

}

//void onUnpair(myo::Myo* myo, uint64_t timestamp) {}

void onArmSync(myo::Myo* myo, uint64_t timestamp, myo::Arm arm,

myo::XDirection xDirection) {


147

knownMyos[getMyoID(myo,timestamp)-1]->addArm(arm,timestamp);

knownMyos[getMyoID(myo,timestamp)-1]->addXDir(xDirection,timestamp);

}

void onArmUnsync(myo::Myo* myo, uint64_t timestamp) {


// infer state changes of arm and xdir

myo::Arm newArm = myo::Arm::armUnknown;

myo::XDirection newXDir = myo::XDirection::xDirectionUnknown;

knownMyos[getMyoID(myo,timestamp)-1]->addArm(newArm,timestamp);

knownMyos[getMyoID(myo,timestamp)-1]->addXDir(newXDir,timestamp);

}

//void onUnlock(myo::Myo* myo, uint64_t timestamp) {}

}; // DataCollector

// END DataCollector

#endif // ndef MYO_CLASS_HPP

Appendix A.2 myo_mex.cpp

// comment the following line to remove debug output via mexPrintf()

//#define DEBUG_MYO_MEX

#ifdef DEBUG_MYO_MEX

#define DB_MYO_MEX(fmt, ...) mexPrintf(fmt, ##__VA_ARGS__)

#else

#define DB_MYO_MEX(fmt, ...)

#endif

#include <mex.h> // mex api

#include <windows.h> // win api for threading support

#include <process.h> // process/thread support

#include <queue> // standard type for fifo queue

#include "myo/myo.hpp"

#include "myo_class.hpp"

// macros

#define MAKE_NEG_VAL_ZERO(val) (val<0)?(0):(val)

// indeces of output args (into plhs[*])

#define DATA_STRUCT_OUT_NUM 0

// indeces of data fields into data output struct

#define QUAT_FIELD_NUM 0

#define GYRO_FIELD_NUM 1

#define ACCEL_FIELD_NUM 2

#define POSE_FIELD_NUM 3

#define ARM_FIELD_NUM 4

#define XDIR_FIELD_NUM 5

#define EMG_FIELD_NUM 6

#define NUM_FIELDS 7

const char* output_fields[] =

{"quat","gyro","accel","pose","arm","xDir","emg"};

// program behavior parameters

#define STREAMING_TIMEOUT 5

148

#define INIT_DELAY 1000

#define RESTART_DELAY 500

#define READ_BUFFER 2

// program state

volatile bool runThreadFlag = false;

// global data

DataCollector collector;

myo::Hub* pHub = NULL;

myo::Myo* pMyo = NULL;

unsigned int countMyosRequired = 1;

// threading

unsigned int threadID;

HANDLE hThread;

HANDLE hMutex;

// thread routine

unsigned __stdcall runThreadFunc( void* pArguments ) {

while ( runThreadFlag ) { // unset isStreaming to terminate thread

// acquire lock then write data into queue

DWORD dwWaitResult;



{



pHub->runOnce(STREAMING_TIMEOUT); // run callbacks to collector


if (! ReleaseMutex(hMutex)) { return FALSE; } // acquired bad mutex

break;


return FALSE; // acquired bad mutex

}

} // end thread and return

_endthreadex(0); //

return 0;

}

// These functions allocate and assign mxArray to return output to MATLAB

// Pseudo example usage:

// mxArray* outData[...];

// makeOutputXXX(outData,...);

// fillOutputXXX(outData,...);

// // then assign matrices in outData to a MATLAB struct

// plhs[...] = mxCreateStructMatrix(...);

// assnOutputStruct(plhs[...],outData,...);

// Note: The size of outData must be consistent with hard code in the

// makeOutdataXXX and fillOutdataXXX functions.

void makeOutputIMU(mxArray *outData[], unsigned int sz) {

outData[QUAT_FIELD_NUM] =

mxCreateNumericMatrix(sz,4,mxDOUBLE_CLASS,mxREAL);

outData[GYRO_FIELD_NUM] =


outData[ACCEL_FIELD_NUM] =

149


outData[POSE_FIELD_NUM] =


outData[ARM_FIELD_NUM] =


outData[XDIR_FIELD_NUM] =


}

void makeOutputEMG(mxArray *outData[], unsigned int sz) {

outData[EMG_FIELD_NUM] =


}

void fillOutputIMU(mxArray *outData[], FrameIMU f,

unsigned int row,unsigned int sz) {

*( mxGetPr(outData[QUAT_FIELD_NUM]) + row+sz*0 ) = f.quat.w();

*( mxGetPr(outData[QUAT_FIELD_NUM]) + row+sz*1 ) = f.quat.x();

*( mxGetPr(outData[QUAT_FIELD_NUM]) + row+sz*2 ) = f.quat.y();

*( mxGetPr(outData[QUAT_FIELD_NUM]) + row+sz*3 ) = f.quat.z();

*( mxGetPr(outData[GYRO_FIELD_NUM]) + row+sz*0 ) = f.gyro.x();

*( mxGetPr(outData[GYRO_FIELD_NUM]) + row+sz*1 ) = f.gyro.y();

*( mxGetPr(outData[GYRO_FIELD_NUM]) + row+sz*2 ) = f.gyro.z();

*( mxGetPr(outData[ACCEL_FIELD_NUM]) + row+sz*0 ) = f.accel.x();

*( mxGetPr(outData[ACCEL_FIELD_NUM]) + row+sz*1 ) = f.accel.y();

*( mxGetPr(outData[ACCEL_FIELD_NUM]) + row+sz*2 ) = f.accel.z();

*( mxGetPr(outData[POSE_FIELD_NUM]) + row ) = f.pose.type();

*( mxGetPr(outData[ARM_FIELD_NUM]) + row ) = f.arm;

*( mxGetPr(outData[XDIR_FIELD_NUM]) + row ) = f.xDir;

}

void fillOutputEMG(mxArray *outData[], FrameEMG f,

unsigned int row,unsigned int sz) {

int jj = 0;

for (jj;jj<8;jj++)

*( mxGetPr(outData[EMG_FIELD_NUM]) + row+sz*jj ) = f.emg[jj];

}

void assnOutputStruct(mxArray *s, mxArray *d[], int id) {

int ii = 0;

for (ii;ii<NUM_FIELDS;ii++) {

DB_MYO_MEX("Setting field %d of struct element %d\n",ii+1,id);

mxSetFieldByNumber(s,id-1,ii,d[ii]);

}

}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])

{

// check for proper number of arguments

if( nrhs<1 )

mexErrMsgTxt("myo_mex requires at least one input.");

if ( !mxIsChar(prhs[0]) )

mexErrMsgTxt("myo_mex requires a char command as the first input.");

if(nlhs>1)

mexErrMsgTxt("myo_mex cannot provide the specified number of outputs.");

char* cmd = mxArrayToString(prhs[0]);

if ( !strcmp("init",cmd) ) {

150

// ----------------------------------------- myo_mex init -------------

if ( mexIsLocked() )

mexErrMsgTxt("myo_mex is already initialized.\n");

if ( nrhs<2 )

mexErrMsgTxt("myo_mex init requires 2 inputs.\n");

if( !mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) ||

!(mxGetM(prhs[1])==1 && mxGetM(prhs[1])==1) )

mexErrMsgTxt("myo_mex init requires a numeric scalar countMyos as the

second input.");

// Get input counyMyos and set up collector accordingly

countMyosRequired = *mxGetPr(prhs[1]);

if (countMyosRequired==1)

collector.addEmgEnabled = true;

// Instantiate a Hub and get a Myo

pHub = new myo::Hub("com.mark-toma.myo_mex");

if ( !pHub )

mexErrMsgTxt("Hub failed to init!");

pMyo = pHub->waitForMyo(5);

if ( !pMyo )

mexErrMsgTxt("Myo failed to init!");

// configure myo and hub

pHub->setLockingPolicy(myo::Hub::lockingPolicyNone); // TODO: What does

this do?

pHub->addListener(&collector);

// instantiate mutex

hMutex = CreateMutex(NULL,FALSE,NULL);

if (hMutex == NULL)

mexErrMsgTxt("Failed to set up mutex.\n");

// Let Hub run callbacks on collector so we can figure out how many

// Myos are connected to Myo Connect so we can assert countMyosRequired

pHub->run(INIT_DELAY);

if (countMyosRequired!=collector.getCountMyos())

mexErrMsgTxt("myo_mex failed to initialize with countMyos.\n");

// Flush the data queues with syncDataSources

// Note: This pops the oldest samples of data off the front of all

// queues until only the most recent data remains


// At this point we don't anticipate and errors, so we commit to

// locking this file's memory

// Note: The mexLock status is used to determine the initialization

// state in other calls

mexLock();

} else if ( !strcmp("start_streaming",cmd) ) {

// ----------------------------------------- myo_mex start_streaming --




mexErrMsgTxt("myo_mex is already streaming.\n");

if ( nlhs>0 )

151


collector.addDataEnabled = true; // lets collector handle data events

// dispatch concurrent task

runThreadFlag = true;

hThread = (HANDLE)_beginthreadex( NULL, 0, &runThreadFunc, NULL, 0,

&threadID );

if ( !hThread )

mexErrMsgTxt("Failed to create streaming thread!\n");

DB_MYO_MEX("myo_mex start_streaming:\n\tSuccess\n");

} else if ( !strcmp("get_streaming_data",cmd) ) {

// ----------------------------------------- myo_mex get_streaming_data





if ( nlhs>1 )


// Verify that collector still has all of its Myos, otherwise error out


if ( countMyos != countMyosRequired )

mexErrMsgTxt("myo_mex countMyos is inconsistent with initialization...

We lost a Myo!");

// Declarations and initializations and stuff

unsigned int iiIMU1=0; // Index into output matrices when reading queue

unsigned int iiEMG1=0;

unsigned int iiIMU2=0;

unsigned int iiEMG2=0;

unsigned int szIMU1 = 0; // Size of samples to read from queue

unsigned int szEMG1 = 0;

unsigned int szIMU2 = 0;

unsigned int szEMG2 = 0;

FrameIMU frameIMU1, frameIMU2; // Data structures returned from queue

read

FrameEMG frameEMG1, frameEMG2;

// Output matrices hold numeric data



// Compute size of output matrices


if (countMyos<2) {

szEMG1 = collector.getCountEMG(1)-READ_BUFFER;

} else {


}

szIMU1 = MAKE_NEG_VAL_ZERO(szIMU1);

szEMG1 = MAKE_NEG_VAL_ZERO(szEMG1);

szIMU2 = MAKE_NEG_VAL_ZERO(szIMU2);

szEMG2 = MAKE_NEG_VAL_ZERO(szEMG2);

// Initialize output matrices

152





// Now get ahold of the lock and iteratively drain the queue while

// filling outDataN matrices

DWORD dwWaitResult;



{





fillOutputIMU(outData1,frameIMU1,iiIMU1,szIMU1);

iiIMU1++;

}



fillOutputEMG(outData1,frameEMG1,iiEMG1,szEMG1);

iiEMG1++;

}



fillOutputIMU(outData2,frameIMU2,iiIMU2,szIMU2);

iiIMU2++;

}



fillOutputEMG(outData2,frameEMG2,iiEMG2,szEMG2);

iiEMG2++;

}


if ( !ReleaseMutex(hMutex))

mexErrMsgTxt("Failed to release lock\n");

break;


mexErrMsgTxt("Acquired abandoned lock\n");

break;

}

// Assign outDataN matrices to MATLAB struct matrix

plhs[DATA_STRUCT_OUT_NUM] =

mxCreateStructMatrix(1,countMyos,NUM_FIELDS,output_fields);


if (countMyos>1) {


}

} else if ( !strcmp("stop_streaming",cmd) ) {

// ----------------------------------------- myo_mex stop_streaming ---





if ( nlhs>0 )

153


// Terminate thread and reset state

runThreadFlag = false; // thread sees this flag and exits

WaitForSingleObject( hThread, INFINITE );

CloseHandle( hThread );

hThread = NULL;

// Terminate data logging and reset state

collector.addDataEnabled = false; // stop handling data events

collector.syncDataSources(); // sync data up again (flushes queue)

} else if ( !strcmp("delete",cmd) ) {

// ----------------------------------------- myo_mex delete -----------




mexErrMsgTxt("myo_mex cannot be deleted while streaming. Call

stop_streaming first.\n");

if ( nlhs>0 )


CloseHandle (hMutex);

hMutex = NULL;

mexUnlock();

if (pHub!=NULL)

delete pHub;

} else {

mexErrMsgTxt("unknown command!\n");

}

return;

Appendix A.3 SpheroCore.SpinProtocol

function SpinProtocol(s)

% SpinProtocol Reads local input buffer to parse RSP and MSG

% Invoked by the Bluetooth callback BytesAvailableFcn

if length(s.buffer) < 6, return; end % bail if not enough data for a packet

% grab first two bytes and bail on protocol fault

sop1 = s.buffer(1);

sop2 = s.buffer(2);

if sop1 ~= s.SOP1

s.DEBUG_PRINT('Missed SOP1 byte');

s.buffer = s.buffer(2:end);

return;

elseif ~any( sop2 == [s.SOP2_RSP,s.SOP2_ASYNC] )

s.DEBUG_PRINT('Missed SOP2 byte');

s.buffer = s.buffer(3:end);

return;

end

% sop1 and sop2 are both valid now

154

% proceed to read in

if sop2 == s.SOP2_RSP

% response format

% [ sop1 | sop2 | mrsp | seq | dlen | <data> | chk ]

mrsp = s.buffer(3);

seq = s.buffer(4);

dlen = s.buffer(5);

% read data into buffer

% if the whole packet isn't in the buffer yet, this part will

% attempt a blocking read on the assumed missing bytes.

num_bytes = 5+dlen - length(s.buffer);

new_bytes = [];

if num_bytes > 0

new_bytes = fread(s.bt,num_bytes,'uint8')';

end

s.buffer = [s.buffer,new_bytes];

s.num_skip = s.num_skip + num_bytes; % adjustment for BytesAvailableFcn to

ignore the triggers for bytes read manually

% move packet out of buffer

packet = s.buffer(1:5+dlen);

s.buffer = s.buffer(5+dlen+1:end);

% grab data, chk, validate chk

data = packet(6:end-1);

chk = packet(end);

chk_cmp = bitcmp(mod(sum(uint8(packet(3:end-1))),256),'uint8');

if chk ~= chk_cmp, return; end

s.response_packet.sop1 = sop1;

s.response_packet.sop2 = sop2;

s.response_packet.mrsp = mrsp;

s.response_packet.seq = seq;

s.response_packet.dlen = dlen;

s.response_packet.data = data;

s.response_packet.chk = chk;

elseif sop2 == s.SOP2_ASYNC

% async message format

% [ sop1 | sop2 | id_code | dlen_msb | dlen_lsb | <data> | chk ]

id_code = s.buffer(3);

dlen_msb = s.buffer(4);

dlen_lsb = s.buffer(5);

dlen = s.IntegerFromByteArray([dlen_msb,dlen_lsb],'uint16');

% read data into buffer

% if the whole packet isn't in the buffer yet, this part will

% attempt a blocking read on the assumed missing bytes.

num_bytes = double(5+dlen - length(s.buffer));

new_bytes = [];

if num_bytes > 0

new_bytes = fread(s.bt,num_bytes,'uint8')';

end

s.buffer = [s.buffer,new_bytes];

155

s.num_skip = s.num_skip + num_bytes; % adjustment for BytesAvailableFcn to

ignore the triggers for bytes read manually

% move packet out of buffer

packet = s.buffer(1:5+dlen);

s.buffer = s.buffer(5+dlen+1:end);

% grab data, chk, validate chk

data = packet(6:end-1);

chk = packet(end);

chk_cmp = bitcmp(mod(sum(uint8(packet(3:end-1))),256),'uint8');

if chk ~= chk_cmp, return; end

s.DEBUG_PRINT('received packet: %s',sprintf('%0.2X ',packet));

% handle the message

switch id_code

case s.ID_CODE_POWER_NOTIFICATIONS

s.HandlePowerNotification(data);

case s.ID_CODE_LEVEL_1_DIAGNOSTIC_RESPONSE

s.HandleLevel1Diagnostic(data);

case s.ID_CODE_SENSOR_DATA_STREAMING

s.HandleDataStreaming(data);

case s.ID_CODE_CONFIG_BLOCK_CONTENTS

s.HandleConfigBlockContents(data);

case s.ID_CODE_PRE_SLEEP_WARNING

s.HandlePreSleepWarning(data);

case s.ID_CODE_MACRO_MARKERS

s.HandleMacroMarkers(data);

case s.ID_CODE_COLLISION_DETECTED

s.HandleCollisionDetected(data);

case s.ID_CODE_ORB_BASIC_PRINT_MESSAGE

s.HandleOrbBasic(data,'print');

case s.ID_CODE_ORB_BASIC_ERROR_MESSAGE_ASCII

s.HandleOrbBasic(data,'error-ascii');

case s.ID_CODE_ORB_BASIC_ERROR_MESSAGE_BINARY

s.HandleOrbBasic(data,'error-binary');

case s.ID_CODE_SELF_LEVEL_RESULT

s.HandleSelfLevelResult(data);

case s.ID_CODE_GYRO_AXIS_LIMIT_EXCEEDED

s.HandleGyroAxisLimitExceeded(data);

case s.ID_CODE_SPHEROS_SOUL_DATA

s.HandleSpheroSoulData(data);

case s.ID_CODE_LEVEL_UP_NOTIFICATION

s.HandleLevelUpNotification(data);

case s.ID_CODE_SHIELD_DAMAGE_NOTIFICATION

s.HandleShieldDamageNotification(data);

case s.ID_CODE_XP_UPDATE_NOTIFICATION

s.HandleXpUpdateNotification(data);

case s.ID_CODE_BOOST_UPDATE_NOTIFICATION

s.HandleBoostUpdateNotification(data);

otherwise

s.DEBUG_PRINT('Unsupported asyncronous message!');

end

end

end

156

References

[1] MathWorks Image Acquisition Toolbox Team, "Image Acquisition Toolbox

Support Package for Kinect For Windows Sensor - File Exchange - MATLAB

Central," The MathWorks, Inc., 7 March 2013. [Online]. Available:

https://www.mathworks.com/matlabcentral/fileexchange/40445-image-

acquisition-toolbox-support-package-for-kinect-for-windows-sensor.

[Accessed 15 January 2017].

[2] J. R. Terven,

"https://www.mathworks.com/matlabcentral/fileexchange/53439-kinect-2-

interface-for-matlab," The MathWorks, Inc., 9 October 2015. [Online].

Available: https://www.mathworks.com/matlabcentral/fileexchange/53439-

kinect-2-interface-for-matlab. [Accessed 15 January 2017].

[3] J. R. Terven and D. M. Córdova-Esparza, "Kin2. A Kinect 2 toolbox for MATLAB,"

Science of Computer Programming, vol. 130, pp. 97-106, 2016.

[4] A. Boyali and N. Hashimoto, "Spectral Collaborative Representation based

Classification for hand gestures recognition on electromyography signals,"

Biomedical Signal Processing and Control, vol. 24, pp. 11-18, 2016.

[5] A. Boyali, "boyali/matMYO: Matlab MYO Libray for Raw Acquiring Sensor Data

in Batch Mode," GitHub, Inc., 14 March 2016. [Online]. Available:

https://github.com/boyali/matMYO. [Accessed 15 January 2017].

[6] Y. J. Lee, "Sphero MATLAB Interface - File Exchange - MATLAB Central," The

MathWorks, Inc., 5 Novermber 2014. [Online]. Available:

https://www.mathworks.com/matlabcentral/fileexchange/48359-sphero-

matlab-interface. [Accessed 15 January 2017].

[7] D. Sethi, "Sphero Connectivity Package - File Exchange - MATLAB Central," The

MathWorks, Inc., 25 August 2015. [Online]. Available:

https://www.mathworks.com/matlabcentral/fileexchange/52481-sphero-

connectivity-package. [Accessed 15 January 2017].

[8] B. Stern, "Inside Myo | Myo Armband Teardown | Adafruit Learning System,"

Adafruit Industries, 3 February 2016. [Online]. Available:

https://learn.adafruit.com/myo-armband-teardown/inside-myo. [Accessed 6

January 2017].

157

[9] developer.thalmic.com, "Thalmic Labs - Maker of Myo gesture control

armband," Thalmic Labs, 2016. [Online]. Available:

https://developer.thalmic.com/downloads. [Accessed 8 December 2016].

[10] developer.thalmic.com, "Thalmic Labs Developer Forum / Tools and Bindings /

List of Unofficial Tools and Bindings," Thalmic Labs, 2016. [Online]. Available:

https://developer.thalmic.com/forums/topic/541/. [Accessed 8 December

2016].

[11] thalmiclabs, "myo-bluetooth/myohw.h at master · thalmiclabs/myo-

bluetooth," 28 September 2015. [Online]. Available:

https://github.com/thalmiclabs/myo-bluetooth/blob/master/myohw.h.

[Accessed 8 December 2016].

[12] E. White, "Disassembling BB8 (Part 2) | element14 | chriswhite," Element 14: A

Premier Farnell Company, 17 September 2015. [Online]. Available:

https://www.element14.com/community/blogs/linker/2015/09/17/disasse

mbling-bb8-part2. [Accessed 6 January 2017].

[13] sdk.sphero.com, "Sphero Docs | Getting Started," Sphero, 2016. [Online].

Available: http://sdk.sphero.com/sdk-documentation/getting-started/.


[14] M. Tomaszewski, "Myo SDK MATLAB MEX Wrapper," The MathWorks, Inc., 7

March 2016. [Online]. Available:

https://www.mathworks.com/matlabcentral/fileexchange/55817-myo-sdk-

matlab-mex-wrapper. [Accessed 7 January 2017].

[15] M. Tomaszewski, "Sphero API MATLAB SDK - File Exchange - MATLAB

Central," The MathWorks, Inc., 30 August 2015. [Online]. Available:

https://www.mathworks.com/matlabcentral/fileexchange/52746-sphero-api-

matlab-sdk. [Accessed 7 January 2017].

[16] M. Tomaszewski, "mark-toma/MyoMex: Access data from Thalmic Labs' Myo

Gesture Control Armband in m-code!," GitHub, 20 November 2016. [Online].

Available: https://github.com/mark-toma/MyoMex. [Accessed 26 December

2016].

158

[17] developer.thalmic.com, "Myo SDK 0.9.0: Myo SDK Manual," Thalmic Labs, 2014.

[Online]. Available:

https://developer.thalmic.com/docs/api_reference/platform/index.html.


[18] M. Tomaszewski, "mark-toma/SpheroMATLAB: Control Sphero from MATLAB

in m-code!," GitHub, Inc., 17 August 2016. [Online]. Available:

https://github.com/mark-toma/SpheroMATLAB. [Accessed 26 December

2016].

[19] Orbotix, "Sphero API 1.50," 20 August 2013. [Online]. Available:

https://github.com/orbotix/DeveloperResources/blob/master/docs/Sphero_

API_1.50.pdf. [Accessed 8 January 2017].