Recommended
More Related Content
Similar to Tomaszewski, Mark - Thesis Slides: Application of Consumer-Off-The-Shelf (COTS) Devices to Human Motion Analysis
Similar to Tomaszewski, Mark - Thesis Slides: Application of Consumer-Off-The-Shelf (COTS) Devices to Human Motion Analysis (20)
Recently uploaded
Recently uploaded (20)
Tomaszewski, Mark - Thesis Slides: Application of Consumer-Off-The-Shelf (COTS) Devices to Human Motion Analysis
- 1. APPLICATION OF CONSUMER-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
- 2. Mark Tomaszewski13 January 2017 Slide 2 of 64 WHO AM I? Two Years in ARMLAB Commercial motion capture evaluation Pelvic floor force estimation Motion capture experiments Robotic systems Fall 2014 … … February 2017 Model based vehicle dynamics
- 3. 13 January 2017 Mark Tomaszewski Slide 3 of 64 INTRODUCTION
- 4. Mark Tomaszewski13 January 2017 Slide 4 of 64 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
- 5. Mark Tomaszewski13 January 2017 Slide 5 of 64 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
- 6. Mark Tomaszewski13 January 2017 Slide 6 of 64 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)
- 7. Mark Tomaszewski13 January 2017 Slide 7 of 64 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
- 8. 13 January 2017 Mark Tomaszewski Slide 8 of 64 BACKGROUND SOFTWARE DEVELOPMENT MATHEMATICAL METHODS MOTION ANALYSIS DISCUSSION
- 9. Mark Tomaszewski13 January 2017 Slide 9 of 64 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 module Motors MCU IMU Motor driver
- 10. Mark Tomaszewski13 January 2017 Slide 10 of 64 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
- 11. Mark Tomaszewski13 January 2017 Slide 11 of 64 BACKGROUND Myo Software – Development Ecosystem 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 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.
- 12. Mark Tomaszewski13 January 2017 Slide 12 of 64 BACKGROUND Myo Software – Middleware Stack Myo Device (Embedded Host Device) Application Computer (Windows Client PC) Platform SDK API Middleware 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]
- 13. Mark Tomaszewski13 January 2017 Slide 13 of 64 BACKGROUND Myo Software – API selection 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 Platform SDK API Middleware MATLAB Interface Myo SDK Implementation myo_mex (MATLAB MEX Wrapper) MyoMex Device Manager (MATLAB Class) MyoData Device Data Interface (MATLAB Class) Myo SDK MATLAB MEX Wrapper [6,7]
- 14. Mark Tomaszewski13 January 2017 Slide 14 of 64 BACKGROUND Sphero Software – Support Operating System Language Dependencies Supported By iOS 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]
- 15. 13 January 2017 Mark Tomaszewski Slide 15 of 64 BACKGROUND SOFTWARE DEVELOPMENT MATHEMATICAL METHODS MOTION ANALYSIS DISCUSSION
- 16. Mark Tomaszewski13 January 2017 Slide 16 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 collector with 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.
- 17. Mark Tomaszewski13 January 2017 Slide 17 of 64 myo_class.hpp SOFTWARE DEVELOPMENT Myo SDK Implementation • 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
- 18. Mark Tomaszewski13 January 2017 Slide 18 of 64 SOFTWARE DEVELOPMENT 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 Mutex Read Data Release Mutex get_streaming_data End Thread Begin Thread mexLock() mexUnlock()
- 19. Mark Tomaszewski13 January 2017 Slide 19 of 64 SOFTWARE DEVELOPMENT Myo MEX Implementation myo_mex.cpp
- 20. Mark Tomaszewski13 January 2017 Slide 20 of 64 %% Collect 5s of EMG data mm = MyoMex(); pause(5); emg = mm.myoData.emg_log; mm.delete(); clear mm; SOFTWARE DEVELOPMENT Myo MATLAB Implementation • MyoMex class manages myo_mex() • MATLAB timer used to schedule data polling • Calls into myo_mex() to fetch new data • MyoData class manages data • Owned by MyoMex • Vectorized for multiple Myos MyoMex myo_mex(‘init’) Start Timer Instantiation 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’)
- 21. Mark Tomaszewski13 January 2017 Slide 21 of 64 SOFTWARE DEVELOPMENT Myo MATLAB Implementation MyoMex.m
- 22. Mark Tomaszewski13 January 2017 Slide 22 of 64 SOFTWARE DEVELOPMENT Myo MATLAB Implementation MyoData.m
- 23. Mark Tomaszewski13 January 2017 Slide 23 of 64 SOFTWARE DEVELOPMENT 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 DLEN MSB DLEN LSB <DATA> CHK
- 24. Mark Tomaszewski13 January 2017 Slide 24 of 64 SOFTWARE DEVELOPMENT 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 58h CMD SetDataStreaming() – acknowledgement FFh FFh 00h 37h 01h C7h RSP SetDataStreaming() – streaming data message FFh FEh 03h 00h 0Dh 00h 00h 00h 0Ah 00h FBh FFh DFh 00h 76h 10h 24h 58h MSG T I M E Filtered: −0.01 0.03 1.01 𝑔 Raw: 0 0.04 0.98 𝑔
- 25. Mark Tomaszewski13 January 2017 Slide 25 of 64 SOFTWARE DEVELOPMENT Sphero MATLAB Implementation Receiving RSP and MSG Return New Data BytesAvailableFcn() SpinProtocol() num_bytes > 0 Yes No RSP packet MSG packet No 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);
- 26. Mark Tomaszewski13 January 2017 Slide 26 of 64 SOFTWARE DEVELOPMENT Sphero MATLAB Implementation Sending CMD Return false API Function WriteClientCommandPacket() WaitForCommandResponse() answer_flag No Yes response_packet No Yes No Validate response_packet Yes 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’);
- 27. Mark Tomaszewski13 January 2017 Slide 27 of 64 SOFTWARE DEVELOPMENT Sphero MATLAB Implementation SpheroCore.m & SpheroCoreConstants.m
- 28. Mark Tomaszewski13 January 2017 Slide 28 of 64 SOFTWARE DEVELOPMENT Sphero Interface • SpheroInterface inherits from SpheroCore • Overloads and extends • Right-handed coordinates Roll() 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
- 29. Mark Tomaszewski13 January 2017 Slide 29 of 64 SOFTWARE DEVELOPMENT 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();
- 30. Mark Tomaszewski13 January 2017 Slide 30 of 64 SOFTWARE DEVELOPMENT Myo Graphical User Interface LIVE DEMO Demo script relative path: demodemo_script.m
- 31. Mark Tomaszewski13 January 2017 Slide 31 of 64 SOFTWARE DEVELOPMENT Myo GUI – Backup Quaternion Gyroscope Accelerometer EMG Pose Spatial Quaternion Visualization Time history strip charts
- 32. Mark Tomaszewski13 January 2017 Slide 32 of 64 SOFTWARE DEVELOPMENT Myo GUI – Backup https://www.youtube.com/watch?v=pPh306IgEDo
- 33. Mark Tomaszewski13 January 2017 Slide 33 of 64 SOFTWARE DEVELOPMENT Sphero GUI Odometry Data Vector DriveConnection Dialogue Gyroscope Accelerometer Spatial Quaternion Visualization Time history strip charts
- 34. Mark Tomaszewski13 January 2017 Slide 34 of 64 SOFTWARE DEVELOPMENT Sphero GUI – Backup https://www.youtube.com/watch?v=YohxMa_z4Ww
- 35. Mark Tomaszewski13 January 2017 Slide 35 of 64 SOFTWARE DEVELOPMENT Myo and Sphero Application https://www.youtube.com/watch?v=4TJzZF22GnA
- 36. 13 January 2017 Mark Tomaszewski Slide 36 of 64 BACKGROUND SOFTWARE DEVELOPMENT MATHEMATICAL METHODS MOTION ANALYSIS DISCUSSION
- 37. Mark Tomaszewski13 January 2017 Slide 37 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
- 38. Mark Tomaszewski13 January 2017 Slide 38 of 64 MATHEMATICAL METHODS 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 𝑹 𝑠 𝑁𝑠 𝐹 𝑁𝑠 𝑠 𝑹 𝑠 𝐹 𝑹 𝐹 𝑁𝑠 𝑹 𝑠 𝑁𝑠
- 39. Mark Tomaszewski13 January 2017 Slide 39 of 64 MATHEMATICAL METHODS 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 𝑹 𝐻 𝑁 𝐻
- 40. Mark Tomaszewski13 January 2017 Slide 40 of 64 MATHEMATICAL METHODS 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 0 0 𝑐 𝜃 −𝑠 𝜃 0 𝑠 𝜃 𝑐 𝜃 𝑴 𝑦 𝜃 = 𝑐 𝜃 0 𝑠 𝜃 0 1 0 −𝑠 𝜃 0 𝑐 𝜃 𝑴 𝑧 𝜃 = 𝑐 𝜃 −𝑠 𝜃 0 𝑠 𝜃 𝑐 𝜃 0 0 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 𝑧−𝑦−𝑥
- 41. Mark Tomaszewski13 January 2017 Slide 41 of 64 MATHEMATICAL METHODS 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 𝑥 − 𝑦 − 𝑧 𝑹 𝐻 𝐿 𝜃 𝑤𝑥 𝜃 𝑤𝑦 𝜃 𝑤𝑧
- 42. Mark Tomaszewski13 January 2017 Slide 42 of 64 MATHEMATICAL METHODS 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 𝒅 𝐶 𝑖 𝐹 𝝐
- 43. Mark Tomaszewski13 January 2017 Slide 43 of 64 • Start with realization 𝑘 at point 𝑖 • Write for 𝑘 ∈ 𝐾 = 1,2, … , 𝑃𝑖 • Write for 𝑖 = 1,2,3 and assemble • Objective function 𝑓 𝒙 = 𝝐 𝐺 T 𝝐 𝐺 = 𝒙T 𝑨 𝐺 T 𝑨 𝐺 𝒙 − 2𝒃 𝐺 T 𝑨 𝐺 𝒙 + 𝒃 𝐺 T 𝒃 𝐺 MATHEMATICAL METHODS 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 𝒍 𝒅 𝐶 𝑖 𝐹 − 𝒃 𝑘𝑖
- 44. Mark Tomaszewski13 January 2017 Slide 44 of 64 MATHEMATICAL METHODS 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
- 45. Mark Tomaszewski13 January 2017 Slide 45 of 64 MATHEMATICAL METHODS Calibration – Constraints Distance ℎ 𝐷 𝑖𝑗 = 𝒓 𝐶 𝑖/ 𝐶 𝑗 T 𝒓 𝐶 𝑖/ 𝐶 𝑗 − 𝑟 𝐶 𝑖/ 𝐶 𝑗 2 = 0, 𝑖, 𝑗 = 2,1 , 2,3 , 3,1 ℎ 𝐷21 = 1 2 𝒙T 𝑸21 𝒙 − 𝑟 𝐶2/ 𝐶1 2 ℎ 𝐷23 = 1 2 𝒙T 𝑸23 𝒙 − 𝑟 𝐶2/ 𝐶3 2 ℎ 𝐷31 = 1 2 𝒙T 𝑸31 𝒙 − 𝑟 𝐶3/ 𝐶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
- 46. Mark Tomaszewski13 January 2017 Slide 46 of 64 MATHEMATICAL METHODS Calibration – Constraints 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 𝑺 𝑧 = 𝒆 𝑧
- 47. Mark Tomaszewski13 January 2017 Slide 47 of 64 MATHEMATICAL METHODS 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 Vertical 1 Quadratic Equality 𝑔 𝑉 1 Quadratic Inequality minimize 𝒙 𝑓 𝒙 subject to 𝒈 𝐵 ≼ 0 𝑔 𝑉 ≤ 0 ℎ 𝐷21 = 0 ℎ 𝐷23 = 0 ℎ 𝐷31 = 0 ℎ 𝑂 = 0
- 48. Mark Tomaszewski13 January 2017 Slide 48 of 64 MATHEMATICAL METHODS Experiment Protocol Timed progression through states home, calib, and reach State Location Duration home t-pose --- idle --- 2s calib 𝐶1 3s idle --- 2s reach 𝐶1 → 𝐶2 3s idle --- 2s calib 𝐶2 3s idle --- 2s reach 𝐶2 → 𝐶3 3s idle --- 2s calib 𝐶3 3s idle --- 2s reach 𝐶3 → 𝐶1 3s idle --- 2s calib 𝐶2 3s
- 49. Mark Tomaszewski13 January 2017 Slide 49 of 64 MATHEMATICAL METHODS Experiment Analysis • Plane error 𝒆 𝑝 = 𝒓 𝑆/ 𝑃 = 𝒏𝑖𝑗 T 𝒓 𝑆/ 𝐶 𝑖 𝒏𝑖𝑗 • Disparity of 𝒅 𝑆 𝐹 calculated from inverse kinematics solution 𝐹 𝐶𝑗 𝐶𝑖 𝑆 𝑃 𝒓 𝐶 𝑖/ 𝐶 𝑗 𝒏𝑖𝑗 𝒅 𝑆 𝐹 𝒅 𝐶 𝑖 𝐹 𝒅 𝐶 𝑗 𝐹 𝒓 𝑃/𝐶 𝑖 𝒓 𝑆/𝑃 𝒓 𝑆/𝐶 𝑖 𝑇𝒛 𝑇 𝒏𝑖𝑗 = 𝒓 𝐶 𝑖/ 𝐶 𝑗 × 𝒛 𝑇 𝒓 𝐶 𝑖/ 𝐶 𝑗 × 𝒛 𝑇 𝒅 𝑆 𝐹 = 𝒅 𝐶 𝑖 𝐹 + 𝒓 𝑆/ 𝐶𝑖
- 50. 13 January 2017 Mark Tomaszewski Slide 50 of 64 BACKGROUND SOFTWARE DEVELOPMENT MATHEMATICAL METHODS MOTION ANALYSIS DISCUSSION
- 51. Mark Tomaszewski13 January 2017 Slide 51 of 64 MOTION ANALYSIS Experimental Setup Calibration Fixture Experiment Environment Calibration Jig 𝐹 𝑇 𝐶3 𝐶2 𝐶1 𝒅 𝑇 𝐹
- 52. Mark Tomaszewski13 January 2017 Slide 52 of 64 MOTION ANALYSIS Data Collection https://www.youtube.com/watch?v=uJLlz2ibIYs
- 53. Mark Tomaszewski13 January 2017 Slide 53 of 64 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 Violation 1 19 624005 0.00183 3.78E-08 2 23 539047 0.00021 9.54E-09 3 20 533839 0.00204 4.64E-08 4 24 554550 0.00357 5.12E-08 5 20 567094 0.00181 7.14E-09 https://www.youtube.com/watch?v=uJLlz2ibIYs
- 54. Mark Tomaszewski13 January 2017 Slide 54 of 64 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 minimum mean maximum 300 322 347 206 221 239 55 74 87 603 617 627 44%15% 4%range/mean
- 55. Mark Tomaszewski13 January 2017 Slide 55 of 64 MOTION ANALYSIS Data Calibration https://www.youtube.com/watch?v=uJLlz2ibIYs
- 56. Mark Tomaszewski13 January 2017 Slide 56 of 64 MOTION ANALYSIS Data Analysis Results Plane Error 𝑒 𝑝 = 𝒆 𝑝 • Bounds: 8cm, 4cm • Effect of calibration error 𝐶𝑖𝐶𝑗 𝑒 𝑝 𝐶𝑖𝐶𝑗 𝑒 𝑝 4cm 8cm
- 57. Mark Tomaszewski13 January 2017 Slide 57 of 64 MOTION ANALYSIS Data Analysis Results Inverse Kinematics • Joint angles valid, 𝜃𝑒𝑦 ≈ 0 • Reconstruction error bounds: 3.5cm 3.5cm 𝜃𝑒𝑦 ≈ 0
- 58. 13 January 2017 Mark Tomaszewski Slide 58 of 64 BACKGROUND SOFTWARE DEVELOPMENT MATHEMATICAL METHODS MOTION ANALYSIS DISCUSSION
- 59. Mark Tomaszewski13 January 2017 Slide 59 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
- 60. Mark Tomaszewski13 January 2017 Slide 60 of 64 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
- 61. Mark Tomaszewski13 January 2017 Slide 61 of 64 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
- 62. Mark Tomaszewski13 January 2017 Slide 62 of 64 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].
- 63. 13 January 2017 Mark Tomaszewski Slide 63 of 64 THANK YOU!
- 64. Mark Tomaszewski13 January 2017 Slide 64 of 64 CONTACT Email: mark@mark-toma.com | marktoma@buffalo.edu 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