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

APPLICATION OFCONSUMER-OFF-THE-SHELF (COTS) DEVICES TO

HUMAN MOTION ANALYSIS

A Case Study in Proof-of-Concept Development

Mark Tomaszewski

February 2017

Master of Science

Department of Mechanical and Aerospace Engineering

University at Buffalo, State University of New York

Presented on 13 January 2017

Mark Tomaszewski13 January 2017 Slide 2 of 64

WHO AM I?

Two Years in ARMLAB

Commercial motion capture evaluation

Pelvic floorforce estimation Motion capture

experiments

Robotic systems

Fall 2014 …

… February 2017

Model basedvehicle dynamics

13 January 2017

Mark Tomaszewski

Slide 3 of 64

INTRODUCTION


INTRODUCTION

Motivation

Human Motion Analysis

• Application domains• Skills training

• Rehabilitation therapy

• Advantages• Frequency

• Standardization

• Larger-scale deployment

• Requirements• Availability

• Deployability

• Accessibility

• Technical

Consumer Devices

• Available• Commercial supply chain

• Deployable• Commercial distribution

• Accessible• In use by target markets

• Low cost

• Technical challenges• Data quality

• Manufacturing variations

• Repeatability


INTRODUCTION

Survey of Motion Capture Systems

Name Sensing Modality Cost Magnitude

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


INTRODUCTION

Consumer Devices – Comparison

Kinect (vision)

• Requires line of sight indoors

• Installed fixed to task space

• Data: translation, joint positions

• Errors caused by:• Occlusions

• Rotation parallel to image plane

Myo & Sphero (contact – IMU)

• Requires low EMI

• Installed fixed to subject

• Data: rotation, link orientations

• Errors caused by:• Sensor drift

• EMI (e.g. Myo vibration motor)


INTRODUCTION

Outline

• BACKGROUND

• A brief introduction to Myo and Sphero

• SOFTWARE DEVELOPMENT

• MATLAB interface development for Myo and Sphero

• MATHEMATICAL METHODS

• Sensor data, kinematic modeling, model calibration, experiment analysis

• MOTION ANALYSIS

• Experiment implementation and analysis of results

• DISCUSSION

13 January 2017

Mark Tomaszewski

Slide 8 of 64

BACKGROUNDSOFTWARE DEVELOPMENTMATHEMATICAL METHODSMOTION ANALYSISDISCUSSION


BACKGROUND

Myo and Sphero Hardware

Myo [1] (Thalmic Labs)

• Cortex M4 MCU

• MPU-9150 IMU (9 axis + DMP)

• 8 electromyography (EMG)

• Bluetooth Low Energy (BLE)

Sphero [2] (Sphero/Orbotix)

• Cortex M4 MCU

• Bosch BMI055 IMU (6 axis)

• Motors & Motor Driver

• Bluetooth Classic

Main board and batteries

EMG

IMUMCU

Bluetooth moduleMotors

MCU

IMU

Motor driver


BACKGROUND

Myo and Sphero Firmware

Myo

• BLE GATT server

• Few configuration commands• Set lock policy, vibrate, …

• Data notify characteristics• Quaternion

• Gyroscope

• Accelerometer

• EMG

• Other state: pose, arm, xDir

Sphero

• Serial command/control server

• Many functions• Ping, Roll, ReadLocator,

SetDataStreaming

• Streaming data sources• Quaternion

• Gyroscope

• Accelerometer

• Odometry (position, velocity)

• Motor PWM/EMF


BACKGROUND

Myo Software – Development Ecosystem

Operating System Language Dependencies Supported By

Windows C++ Myo SDKruntime library

Thalmic Labs

Mac OS X C++ Myo SDKframework

Thalmic Labs

iOS Objective-C MyoKitframework

Thalmic Labs

Android Java Java Library Thalmic Labs

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

Software developed by Thalmic Labs [3] and Community [4] sources.


BACKGROUND

Myo Software – Middleware Stack

Myo Device(Embedded Host Device)

Application Computer(Windows Client PC)

Platform SDK APIMiddleware

Low-Level API Middleware

MATLAB Interface

Embedded Application

Bluetooth Radio

Bluetooth Dongle(BLED112 Driver)

Myo Connect(Desktop Application)

Myo SDK(libmyo / C++ Bindings)

Myo SDK Implementation

Myo Data Provider

Bluetooth Radio

Bluetooth Library

Myo Data Consumer

myo_mex (MATLAB MEX Wrapper)

MyoMex Device Manager (MATLAB Class)

MyoData Device Data Interface (MATLAB Class)

User Software Vendor Software

Bluetooth Protocol [5]Bluetooth Protocol [5]


BACKGROUND

Myo Software – API selection

Advantages DisadvantagesMyo SDK Vendor support

Hardware included with Myo No EMG data with multiple

Myo devicesBLE Protocol Free choice for all hardware and

software Code volume and complexity Not as easily deployable

Platform SDK API Middleware

MATLAB Interface


myo_mex (MATLAB MEX Wrapper)

MyoMex Device Manager (MATLAB Class)

MyoData Device Data Interface (MATLAB Class)

Myo SDK MATLAB MEX Wrapper [6,7]


BACKGROUND

Sphero Software – Support

Operating System Language Dependencies Supported ByiOS Objective-C RobotKit SDK framework Sphero

iOS Swift RobotKit SDK framework Sphero

Android Java RobotLibrary SDK jar library Sphero

--- Javascript Source code Community

Sphero Embedded Device

MATLAB Interface Application Computer(Mobile Host Device)

Embedded Application

Bluetooth Radio

Sphero (MATLAB Class)

SpheroCore (MATLAB Class)Sphero Low-Level Protocol

SpheroInterface (MATLAB Class)

ICInterface/Bluetooth (MATLAB Class)MATLAB Instrument Control Toolbox

Android / iOS SDKs

User Software Vendor SoftwareBluetooth Protocol [8]

Sphero API MATLAB SDK [9,10]

13 January 2017

Mark Tomaszewski

Slide 15 of 64



onRssi

onLock

onUnlock

onPair

onUnpair

onArmSync

onArmUnsync

onConnect

onDisconnect

onBatteryLevelReceived

onWarmupCompleted

onOrientationData

onAccelerometerData

onGyroscopeData

onEmgData

onPose

Callbacks

SOFTWARE DEVELOPMENT

Myo SDK [11] Concepts

• Event based API

• Application class DataCollector collector;

• Inherits from myo::DeviceListener

• Implement callbacks for data events, e.g. DataCollector::onEmgData(...,int8_t* emg)

• Register collector as a listener to myo::Hub* pHub;• pHub->addListener(&collector);

• The hub calls back into collectorwith new events

• Run the hub to trigger callbacks• pHub->run(duration) or pHub->runOnce(timeout)

• Boring details not shown here…• Boilerplate, error/validation checking, exception handling, etc.


myo_class.hpp



• DataCollector receives data• Threaded myo::Hub::runOnce()with mutex!

• MyoData manages data• Owned by DataCollector

• Stores data in std::queue<T,std::deque<T>>

• Synchronizes queues

• DataCollector::getFrameXXX()

• Pops the oldest sample of data from IMU or EMG sources

• Reading of data queue with mutex!

• Interpolation of state data on IMU time base• IMU time (50Hz): quat, gyro, accel, pose, arm, xDir

• Synchronization of EMG data with two sample frames• EMG sample rate: 200Hz, frame rate: 100Hz



Myo MEX Implementation

• States: idle, streaming

• Transitions: start_streaming, get_streaming_data, stop_streaming• Plus init/delete to enter/exit the idle state

• Actions: begin/end thread and read data

MEX Function

Idle Streaming

start_streaming

init

delete

stop_streaming

Acquire MutexRead Data

Release Mutex

get_streaming_data

EndThread

BeginThread

mexLock()

mexUnlock()



Myo MEX Implementation

myo_mex.cpp


%% Collect 5s of EMG data

mm = MyoMex();

pause(5);

emg = mm.myoData.emg_log;

mm.delete();

clear mm;


Myo MATLAB Implementation

• MyoMex class managesmyo_mex()

• MATLAB timer usedto schedule data polling

• Calls into myo_mex() to fetchnew data

• MyoData class manages data• Owned by MyoMex

• Vectorized for multiple Myos

MyoMex

myo_mex(‘init’) Start TimerInstantiation

TimerFcn()

myo_mex(‘get_streaming_data’)

Deletion

myoData

myo_mex(‘stop_streaming’)

dataaddData()

MEX

quat

gyro

accel

emg

pose

myo_mex(‘delete’)

myo_mex(‘start_streaming’)




MyoMex.m




MyoData.m



Sphero API [12] Concepts

• Bluetooth SPP = stream of bytes

• Bluetooth protocol: packet structure

• Command (CMD): Client Sphero sets response behavior

• Response (RSP): Sphero Client only in response to a CMD

• Message (MSG): Sphero Client asynchronous

• Examples: Ping(), SetRGBLEDOutput(). Roll(), ReadLocator(), SetDataStreaming()

Packet Header Body CRC

CMD SOP1 SOP2 DID CID SEQ DLEN <DATA> CHK

RSP SOP1 SOP2 MRSP SEQ DLEN <DATA> CHK

MSG SOP1 SOP2 ID CODE DLENMSB

DLENLSB

<DATA> CHK



Sphero API – Communication Example

Client Sphero

SetDataStreaming() – raw & filtered accelerometer, one sample, one frame

FFh FFh 02h 11h 37h 0Ah 01h 90h 00h 01h E0h 00h E0h 00h 01h 58hCMD

SetDataStreaming() – acknowledgement

FFh FFh 00h 37h 01h C7hRSP

SetDataStreaming() – streaming data message

FFh FEh 03h 00h 0Dh 00h 00h 00h 0Ah 00h FBhFFh DFh 00h 76h 10h 24h

58h

MSG

T

I

M

E

Filtered:−0.010.031.01

𝑔

Raw:0

0.040.98

𝑔



Sphero MATLAB Implementation

Receiving RSP and MSG

Return

NewData

BytesAvailableFcn()

SpinProtocol()

num_bytes > 0Yes

No

RSP packet MSG packetNo

Yes

No

Accumulate num_bytes Accumulate num_bytes

Yes

Set response_packet Call MSG Handler

bt = Bluetooth(‘Sphero-WPP,1,...

‘BytesAvailableFcn’,@(src,evt)myFcn(src,evt);




Sending CMD

Return false

APIFunction WriteClientCommandPacket()

WaitForCommandResponse()

answer_flag No

Yes

response_packet No

Yes

No

Validate response_packetYes

Construct CMD packet

timeout

Write CMD packet

RSP data

Return false Return empty

Success Failure UnknownStatus:

pkt_ping = hex2dec({‘ff’,’ff’,’00’,’01’,’37’,’c6’});

fwrite(bt,pkt_ping,’uint8’);




SpheroCore.m & SpheroCoreConstants.m



Sphero Interface

• SpheroInterface inherits from SpheroCore

• Overloads and extends

• Right-handed coordinatesRoll() with negative heading

• heading_offset

RollWithOffset()

ConfigureLocatorWithOffset()

• Sphero inherits from SpheroInterface

• Intended to be user application class

• Add properties like userdata

• Add methods to accomplish specific tasks



Command Line Interfaces

Myo EMG Logger

DURATION = 5; % seconds

mm = MyoMex;

pause(DURATION);

emg = mm.myoData.emg_log;

time = mm.myoData.timeEMG_log;

mm.delete();

Sphero Gyroscope Logger

DURATION = 5; % seconds

RATE = 50; % Hz

sensors = {'gyro_raw','gyro_filt'};

s = Sphero('Sphero-WPP');

s.SetStabilization(false);

s.SetDataStreaming(RATE,RATE,DURATION,sens

ors);

pause(DURATION+1);

t = s.time_log;

gr = s.gyro_raw_log;

gf = s.gyro_filt_log;

s.SetStabilization(true);

s.delete();



Myo Graphical User Interface

LIVE

DEMODemo script relative path: demo\demo_script.m

demo/demo_script.m



Myo GUI – Backup

Quaternion

Gyroscope

Accelerometer

EMG

Pose

SpatialQuaternion

Visualization

Time history strip charts



Myo GUI – Backup

https://www.youtube.com/watch?v=pPh306IgEDo

https://www.youtube.com/watch?v=pPh306IgEDo



Sphero GUI

Odometry Data

Vector DriveConnection Dialogue

Gyroscope

Accelerometer

SpatialQuaternion

Visualization

Time history strip charts



Sphero GUI – Backup

https://www.youtube.com/watch?v=YohxMa_z4Ww

https://www.youtube.com/watch?v=YohxMa_z4Ww



Myo and Sphero Application

https://www.youtube.com/watch?v=4TJzZF22GnA

https://www.youtube.com/watch?v=4TJzZF22GnA

13 January 2017

Mark Tomaszewski

Slide 36 of 64



MATHEMATICAL METHODS

Nomenclature

• Coordinate frame given by a translation and rotation w.r.t. another

• Rotation 𝑹𝑖𝑗

transforms vectors from frame 𝑖 to frame 𝑗

• Displacement 𝑘𝒅𝑖𝑗

to frame 𝑖 from 𝑗 in components of 𝑘, 𝒅𝑖𝑗= 𝑗 𝒅𝑖

𝑗

• Relative position 𝒓𝐴/𝐵 to 𝐴 from 𝐵

• Root frame is the fixed frame 𝐹 with standard basis vectors 𝒆𝑥 𝒆𝑦 and 𝒆𝑧

• 𝒆𝑥 = 1 0 0 T, 𝒆𝑦 = 0 1 0 T, 𝒆𝑥 = 0 0 1 T

• Absolute orientation 𝑹𝑖 = 𝑹𝑖𝐹

• Dot product of 𝒖 and 𝒗 is 𝒖 ⋅ 𝒗 = 𝒖T𝒗 = 𝒗T𝒖

• Cross product of 𝒖 into 𝒗 is 𝒖 × 𝒗 = 𝒖 𝒗

• Where 𝒖 =

0 −𝑢𝑧 𝑢𝑦

𝑢𝑧 0 −𝑢𝑥

−𝑢𝑦 𝑢𝑥 0



Working with Sensor Data

• Quaternion 𝒒 =𝑠𝒗

to rotation matrix 𝑹 𝒒

• 𝑹 𝒒 = 1 − 2𝒗T𝒗 𝑰3×3 + 2 𝒗𝒗T + 𝑠 𝒗

• Set home pose for sensor 𝑠 with inertial frame 𝑁𝑠 w.r.t. fixed frame 𝐹

• 𝑹𝑠𝑁𝑠 = 𝑹𝐹

𝑁𝑠𝑹𝑠𝐹 from loop closure

• Assume home pose given by 𝑹𝑠𝐹 = 𝑹𝑠

𝐹, with 𝑹𝑠𝐹 = 𝑰3×3 in this work

• Capture and store offset 𝑹𝐹𝑁𝑠 = 𝑹𝑠

𝑁𝑠 𝑹𝑠𝐹 T

= 𝑹𝑠𝑁𝑠𝑰3×3 = 𝑹𝑠

𝑁𝑠

• Remove offset to compute 𝑹𝑠𝐹 = 𝑹𝐹

𝑁𝑠T𝑹𝑠

𝑁𝑠

𝐹

𝑁𝑠 𝑠

𝑹𝑠𝐹𝑹𝐹

𝑁𝑠

𝑹𝑠𝑁𝑠



Forward Kinematics

• Uses sensor data directly as a 9 DOF representation

• Point of interest is 𝒅𝑆𝐹 = 𝒍𝑈 + 𝒍𝐿 + 𝒍𝐻 + 𝒓𝑆

• 𝒅𝑆𝐹 = 𝑹𝑈𝒆𝑥𝑙𝑈 + 𝑹𝐿𝒆𝑥𝑙𝐿 + 𝑹𝐻𝒆𝑥𝑙𝐻 − 𝑹𝐻𝒆𝑧𝑟𝑠

• Home pose calibration:

• 𝑰3×3 = 𝑹𝑈𝐹 = 𝑹𝐿

𝐹 = 𝑹𝐻𝐹

Hand

𝒅𝑆𝐹

𝐹

𝒍𝑈

𝒍𝐿

𝒍𝐻

𝒓𝑆

𝑈

𝐿 𝐻

𝑆

Upper arm Lower arm

𝑹𝐹𝑁𝑈 = 𝑹𝑈

𝑁𝑈

𝑹𝐹𝑁𝐿 = 𝑹𝐿

𝑁𝐿

𝑹𝐹𝑁𝐻 = 𝑹𝐻

𝑁𝐻

during home pose

𝑹𝑈 = 𝑹𝐹𝑁𝑈

T𝑹𝑈

𝑁𝑈

𝑹𝐿 = 𝑹𝐹𝑁𝐿

T𝑹𝐿

𝑁𝐿

𝑹𝐻 = 𝑹𝐹𝑁𝐻

T𝑹𝐻

𝑁𝐻



Inverse Kinematics – Euler Angles

• Compute Euler angles from joint rotation matrices 𝑹𝑈𝐹 𝑹𝐿

𝑈 and 𝑹𝐻𝐿

• Euler angle sequence 𝑖 − 𝑗 − 𝑘 rotates the proximal frame intrinsically about the axes 𝑖 𝑗 and 𝑘 through angles 𝜃1 𝜃2 and 𝜃3

• 𝑴𝑖𝑗𝑘 = 𝑴𝑖 𝜃1 𝑴𝑗 𝜃2 𝑴𝑘 𝜃3

• Composed rotation matrix for 𝑥 − 𝑦 − 𝑧 and 𝑧 − 𝑦 − 𝑥

• Calculation of Euler angles for 𝑥 − 𝑦 − 𝑧 and 𝑧 − 𝑦 − 𝑥

𝑴𝑥(𝜃 =1 0 00 𝑐𝜃 −𝑠𝜃0 𝑠𝜃 𝑐𝜃

𝑴𝑦 𝜃 =𝑐𝜃 0 𝑠𝜃0 1 0

−𝑠𝜃 0 𝑐𝜃

𝑴𝑧 𝜃 =𝑐𝜃 −𝑠𝜃 0𝑠𝜃 𝑐𝜃 00 0 1

𝑴𝑧𝑦𝑥 =

𝑐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

𝑴𝑥𝑦𝑧 =

𝑐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 = atan2 −𝑚23, 𝑚33 = atan2 𝑠1𝑐2, 𝑐1𝑐2𝜃2 = asin 𝑚13 = asin 𝑠2𝜃3 = atan2 −𝑚12, 𝑚11 = atan2 −𝑐2𝑠3, −𝑐2𝑐3

𝑥−𝑦−𝑧

𝜃1 = atan2 𝑚21, 𝑚11 = atan2 𝑠1𝑐2, 𝑐1𝑐2𝜃2 = asin −𝑚31 = asin 𝑠2𝜃3 = atan2 𝑚32, 𝑚33 = atan2 𝑐2𝑠3, 𝑐2𝑐3

𝑧−𝑦−𝑥



Inverse Kinematics – Joint Angles

• Reduction from 9 to 7 DOF

• Assume that 𝜃𝑒𝑦 ≈ 0 (physical/anatomical constraint)

• Compute 𝜙𝑒𝑥 = 𝜃𝑒𝑥 + 𝜃𝑤𝑥 to combine rotations about the same axis

• Joint angles: 𝜃𝑠𝑧 , 𝜃𝑠𝑦, 𝜃𝑠𝑥, 𝜃𝑒𝑧, 𝜙𝑒𝑥 𝜃𝑤𝑦, and 𝜃𝑤𝑧

• Reconstruction of forward kinematics

• Compute 𝑴𝑖𝑗𝑘 with 𝜃𝑒𝑦 = 0 for 𝑹𝑈𝐹 𝑹𝐿

𝑈 and 𝑹𝐻𝐿

Joint 𝒊 − 𝒋 − 𝒌 𝑴𝒊𝒋𝒌 𝜽𝟏 𝜽𝟐 𝜽𝟑

Shoulder 𝑧 − 𝑦 − 𝑥 𝑹𝑈𝐹 𝜃𝑠𝑧 𝜃𝑠𝑦 𝜃𝑠𝑥

Elbow 𝑧 − 𝑦 − 𝑥 𝑹𝐿𝑈 𝜃𝑒𝑧 𝜃𝑒𝑦 𝜃𝑒𝑥

Wrist 𝑥 − 𝑦 − 𝑧 𝑹𝐻𝐿 𝜃𝑤𝑥 𝜃𝑤𝑦 𝜃𝑤𝑧



Calibration – Problem Setup

• Determine segment lengths 𝑙𝑈 𝑙𝐿 and 𝑙𝐻 and pose of task frame 𝑇

• Introduce calibration points 𝐶1 𝐶2 and 𝐶3 in physical calibration jig

• Minimize the error vector 𝝐 over many realizations at each of 3 points 𝐶𝑖

𝒅𝐶1

𝐹

𝒅𝐶2

𝐹

𝒅𝐶3

𝐹

𝐹

𝐶1 𝐶3

𝐶2

𝑇

Hand

𝒅𝑆𝐹

𝐹

𝒍𝑈

𝒍𝐿

𝒍𝐻

𝒓𝑆

𝑈

𝐿 𝐻

𝑆

Upper arm Lower arm

𝒅𝐶𝑖

𝐹 𝝐


• Start with realization 𝑘 at point 𝑖

• Write for 𝑘 ∈ 𝐾 = 1,2, … , 𝑃𝑖

• Write for 𝑖 = 1,2,3 and assemble

• Objective function 𝑓 𝒙 = 𝝐𝐺T𝝐𝐺 = 𝒙T𝑨𝐺

T𝑨𝐺𝒙 − 2𝒃𝐺T𝑨𝐺𝒙 + 𝒃𝐺

T𝒃𝐺


Calibration – Objective Function

−𝝐𝐾𝑖 =

𝑨1𝑖 −𝑰⋮ ⋮

𝑨𝑘𝑖 −𝑰⋮ ⋮

𝑨𝑃𝑖𝑖 −𝑰

𝒍𝒅𝐶𝑖

𝐹 −

𝒃1𝑖

⋮𝒃𝑘𝑖

⋮𝒃𝑃𝑖𝑖

= 𝑨𝐾𝑖 −𝑰3𝑃𝑖×3𝒍

𝒅𝐶𝑖

𝐹 − 𝒃𝐾𝑖

−𝝐𝐺 =

𝑨𝐾1 −𝑰3𝑃1×3 0 0

𝑨𝐾2 0 −𝑰3𝑃2×3 0

𝑨𝐾3 0 0 −𝑰3𝑃3×3

𝒍𝒅𝐶1

𝐹

𝒅𝐶2

𝐹

𝒅𝐶3

𝐹

−

𝒃𝐾1

𝒃𝐾2

𝒃𝐾3

= 𝑨𝐺𝒙 − 𝒃𝐺

−𝝐𝑘𝑖= 𝒅𝑆𝐹 − 𝒅𝐶𝑖

𝐹 = 𝑹𝑈𝒆𝑥 𝑹𝐿𝒆𝑥 𝑹𝐻𝒆𝑥 −𝑰3×3

𝑙𝑈𝑙𝐿𝑙𝐻𝒅𝐶𝑖

𝐹

− 𝑹𝐻𝒆𝑧𝑟𝑠 = 𝑨𝑘𝑖 −𝑰3×3𝒍

𝒅𝐶𝑖

𝐹 − 𝒃𝑘𝑖



Calibration – Constraints

• Natural geometrical constraints• Bounds (nonnegative) on length variables 𝒈𝐵 = −𝒍 ≼ 𝟎

• Distance between pairs of calibration points

ℎ𝐷𝑖𝑗 = 𝒓𝐶𝑖/𝐶𝑗

T𝒓𝐶𝑖/𝐶𝑗

− 𝑟𝐶𝑖/𝐶𝑗

2= 0, 𝑖, 𝑗 = 2,1 , 2,3 , 3,1

• Imposed geometrical constraints• Orthogonality in calibration point arrangement

ℎ𝑂 = 𝒓𝐶2/𝐶3

T𝒓𝐶2/𝐶1

= 0

• Orientation of calibration jig𝑔𝑉 = − 𝒆𝑧

T 𝒓𝐶2/𝐶3𝒓𝐶2/𝐶1

≤ 0

𝒓𝐶2/𝐶1

𝐶1

𝐶3

𝐶2

𝒓𝐶2/𝐶3

𝒓𝐶3/𝐶1

𝒏𝑇 = 𝒓𝐶2/𝐶3× 𝒓𝐶2/𝐶1

𝐹

𝒛𝐹𝒓𝐶2/𝐶1

= 𝒅𝐶2

𝐹 − 𝒅𝐶1

𝐹

𝒓𝐶2/𝐶3= 𝒅𝐶2

𝐹 − 𝒅𝐶3

𝐹

𝒓𝐶3/𝐶1= 𝒅𝐶3

𝐹 − 𝒅𝐶1

𝐹

𝒙𝑇

𝐹𝑇

𝒅𝐶2

𝐹 = 𝒅𝑇𝐹 𝐶3

𝐶2 = 𝑂𝑇

𝐶1

𝒚𝑇

𝒛𝑇 , 𝒏𝑇

𝒓𝐶2/𝐶1

𝒓𝐶2/𝐶3




Distance

ℎ𝐷𝑖𝑗 = 𝒓𝐶𝑖/𝐶𝑗

T𝒓𝐶𝑖/𝐶𝑗

− 𝑟𝐶𝑖/𝐶𝑗

2= 0, 𝑖, 𝑗 = 2,1 , 2,3 , 3,1

ℎ𝐷21=

1

2𝒙T𝑸21𝒙 − 𝑟𝐶2/𝐶1

2

ℎ𝐷23=

1


2

ℎ𝐷31=

1


2

𝑸21 = 2

03×3 …⋮ 𝑰3×3 −𝑰3×3

−𝑰3×3 𝑰3×3 ⋮… 03×3 12×12

𝑸23 = 2

03×3 …⋮ 03×3 …

⋮ 𝑰3×3 −𝑰3×3

−𝑰3×3 𝑰3×3 12×12

𝑸31 = 2

03×3 …⋮ 𝑰3×3 ⋮ −𝑰3×3

… 03×3 …−𝑰3×3 ⋮ 𝑰3×3 12×12




Orthogonality ℎ𝑂 = 𝒓𝐶2/𝐶3

T𝒓𝐶2/𝐶1

= 0

Orientation (vertical) 𝑔𝑉 = − 𝒆𝑧T 𝒓𝐶2/𝐶3

𝒓𝐶2/𝐶1≤ 0

ℎ𝑂 =1

2𝒙T𝑸𝑂𝒙 𝑸𝑂 =

03×3 …⋮ 03×3 −𝑰3×3 𝑰3×3

−𝑰3×3 2𝑰3×3 −𝑰3×3

𝑰3×3 −𝑰3×3 03×3

𝑔𝑉 = −1

2𝒙T𝑸𝑉𝒙 𝑸𝑉 =

03×3 …⋮ 03×3 −𝑺𝑧 𝑺𝑧

−𝑺𝑧 03×3 −𝑺𝑧

𝑺𝑧 −𝑺𝑧 03×3

𝑺𝑧 = 𝒆𝑧



Calibration – Summary

Function Name Count Linearity Type𝒈𝐵 Bounds 3 Constant Inequalityℎ𝐷 ⋅⋅

Distance 3 Quadratic Equality

ℎ𝑂 Orthogonality 1 Quadratic Equality𝒉𝑃 Planar 2 Linear Equalityℎ𝐻 ⋅

Normal Vector Horizontal 2 Quadratic Equality

ℎ𝑉 Normal Vector Vertical1 Quadratic Equality

𝑔𝑉 1 Quadratic Inequality

minimize𝒙

𝑓 𝒙

subject to 𝒈𝐵 ≼ 0𝑔𝑉 ≤ 0ℎ𝐷21

= 0

ℎ𝐷23= 0

ℎ𝐷31= 0

ℎ𝑂 = 0



Experiment Protocol

Timed progression through states home, calib, and reach

State Location Durationhome t-pose ---idle --- 2scalib 𝐶1 3sidle --- 2sreach 𝐶1 → 𝐶2 3sidle --- 2scalib 𝐶2 3sidle --- 2sreach 𝐶2 → 𝐶3 3sidle --- 2scalib 𝐶3 3sidle --- 2sreach 𝐶3 → 𝐶1 3sidle --- 2scalib 𝐶2 3s



Experiment Analysis

• Plane error 𝒆𝑝 = 𝒓𝑆/𝑃 = 𝒏𝑖𝑗T𝒓𝑆/𝐶𝑖

𝒏𝑖𝑗

• Disparity of 𝒅𝑆𝐹 calculated from inverse kinematics solution

𝐹

𝐶𝑗

𝐶𝑖

𝑆

𝑃

𝒓𝐶𝑖/𝐶𝑗

𝒏𝑖𝑗

𝒅𝑆𝐹

𝒅𝐶𝑖

𝐹

𝒅𝐶𝑗

𝐹

𝒓𝑃/𝐶𝑖

𝒓𝑆/𝑃

𝒓𝑆/𝐶𝑖

𝑇𝒛𝑇 𝒏𝑖𝑗 =𝒓𝐶𝑖/𝐶𝑗

× 𝒛𝑇

𝒓𝐶𝑖/𝐶𝑗× 𝒛𝑇

𝒅𝑆𝐹 = 𝒅𝐶𝑖

𝐹 + 𝒓𝑆/𝐶𝑖

13 January 2017

Mark Tomaszewski

Slide 50 of 64



MOTION ANALYSIS

Experimental Setup

Calibration Fixture Experiment Environment

Calibration Jig

𝐹

𝑇

𝐶3

𝐶2

𝐶1

𝒅𝑇𝐹


MOTION ANALYSIS

Data Collection

https://www.youtube.com/watch?v=uJLlz2ibIYs



MOTION ANALYSIS

Data Processing & Calibration

• 10 (9) trials choose 5

• Segment data: calib, reach

• Optimize on calib data• Make params struct

• Use fmincon()with SQP

• Successful for all calib data

• Update model with optimal values𝒍 𝒅𝑇

𝐹 and 𝑹𝑇𝐹

Trial Iterations Objective Function 1st Order Optimality Constraint Violation1 19 624005 0.00183 3.78E-082 23 539047 0.00021 9.54E-093 20 533839 0.00204 4.64E-084 24 554550 0.00357 5.12E-085 20 567094 0.00181 7.14E-09




MOTION ANALYSIS

Data Processing & Calibration

Calibration Result Invariants

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

minimummeanmaximum

300322347

206221239

557487

603617627

44%15% 4%range/mean


MOTION ANALYSIS

Data Calibration




MOTION ANALYSIS

Data Analysis Results

Plane Error 𝑒𝑝 = 𝒆𝑝

• Bounds: 8cm, 4cm

• Effect of calibration error

𝐶𝑖𝐶𝑗

𝑒𝑝

𝐶𝑖𝐶𝑗

𝑒𝑝

4cm

8cm


MOTION ANALYSIS

Data Analysis Results

Inverse Kinematics

• Joint angles valid, 𝜃𝑒𝑦 ≈ 0

• Reconstruction error bounds: 3.5cm

3.5cm

𝜃𝑒𝑦 ≈ 0

13 January 2017

Mark Tomaszewski

Slide 58 of 64



DISCUSSION

Summary of Contributions

1. Myo SDK MATLAB MEX Wrapper• First with streaming data

• 21 5 star ratings

• >50 DL/month

• ≈500 DLs

2. Sphero API MATLAB SDK• First full implementation of Sphero API

• 5 star rating

• ≈150 DLs

3. Model for end-to-end development cycle for COTS applications• Software/middleware

• Modeling: calibration, analysis, evaluation

• Implementation of proposed system


DISCUSSION

Challenges and Future Work

• Myo EMG data with multiple devices• Create new middleware supporting one dongle per Myo

• Replace Myo SDK with: BLE radio + protocol + library and data provider

• Sphero code performance synchronization issues• MATLAB Bluetooth object overhead

• Move API implementation into MEX with third party Bluetooth library

• Accuracy of results• Possibly due to Sphero code performance

• Experimental setup configuration of calibration fixtures

• Experimental protocol (t-pose)

• Sensing modality• Incorporate translational information from vision based solution

• Evolve modeling to include Kinect data


ACKNOWLEDGEMENTS

Thank you so very much!

Advisor Committee Members

Colleagues and Labmates:

Matthias Schmid, S.K. Jun, Xiaobo Zhou, Michael Anson, Yin Chi Chen, Suren Kumar, Javad Sovizi, Ali Alamdari, and many more!

Venkat Krovi Ehsan EsfahaniGary Dargush


REFERENCES

[1] 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].

[2] 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/disassembling-bb8-part2. [Accessed 6 January 2017].

[3] 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].

[4] 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].

[5] 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].

[6] 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].

[7] 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].

[8] 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].

[9] 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].

[10] 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].

[11] 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. [Accessed 8 December 2016].

[12] sdk.sphero.com, "Sphero Docs | Getting Started," Sphero, 2016. [Online]. Available: http://sdk.sphero.com/sdk-documentation/getting-started/. [Accessed 8 December 2016].

13 January 2017

Mark Tomaszewski

Slide 63 of 64

THANK YOU!


CONTACT

Email: [email protected] | [email protected]

Web: http://www.mark-toma.com

http://wiki.mark-toma.com

GitHub: https://github.com/mark-toma

MATLAB Central:

https://www.mathworks.com/matlabcentral/profile/authors/4540343-mark-tomaszewski

mailto:[email protected]

mailto:[email protected]

http://www.mark-toma.com/

http://wiki.mark-toma.com/

https://github.com/mark-toma

https://www.mathworks.com/matlabcentral/profile/authors/4540343-mark-tomaszewski

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

Engineering

Transcript of Tomaszewski, Mark - Thesis Slides: Application of Consumer-Off-The-Shelf (COTS) Devices to Human...