├── .gitignore ├── .readthedocs.yml ├── LICENSE ├── README.md ├── docs ├── Makefile ├── make.bat ├── requirements.txt └── source │ ├── conf.py │ ├── index.rst │ ├── installation.rst │ ├── ref │ ├── core.rst │ ├── core │ │ ├── MimuAngles.rst │ │ └── OmcAngles.rst │ ├── imu.rst │ ├── imu │ │ ├── angles.hip_from_frames.rst │ │ ├── angles.rst │ │ ├── calibration.rst │ │ ├── calibration.static.rst │ │ ├── joints.Center.rst │ │ ├── joints.KneeAxis.rst │ │ ├── joints.correct_knee.rst │ │ ├── joints.fixed_axis.rst │ │ ├── joints.rst │ │ ├── orientation.MadgwickAHRS.rst │ │ ├── orientation.OrientationComplementaryFilter.rst │ │ ├── orientation.SSRO.rst │ │ ├── orientation.rst │ │ ├── pykinematics.imu.utility.calc_derivative.rst │ │ ├── pykinematics.imu.utility.quat2matrix.rst │ │ ├── pykinematics.imu.utility.quat_conj.rst │ │ ├── pykinematics.imu.utility.quat_inv.rst │ │ ├── pykinematics.imu.utility.quat_mean.rst │ │ ├── pykinematics.imu.utility.quat_mult.rst │ │ ├── pykinematics.imu.utility.vec2quat.rst │ │ └── utility.rst │ ├── index.rst │ ├── omc.rst │ └── omc │ │ ├── angles.hip.rst │ │ ├── angles.rst │ │ ├── calibration.compute_hip_center.rst │ │ ├── calibration.rst │ │ ├── calibration.static.rst │ │ ├── pykinematics.omc.utility.compute_pelvis_origin.rst │ │ ├── pykinematics.omc.utility.compute_shank_origin.rst │ │ ├── pykinematics.omc.utility.compute_thigh_origin.rst │ │ ├── pykinematics.omc.utility.create_cluster_frame.rst │ │ ├── pykinematics.omc.utility.default_marker_names.rst │ │ ├── segmentFrames.pelvis.rst │ │ ├── segmentFrames.rst │ │ ├── segmentFrames.thigh.rst │ │ └── utility.rst │ └── usage.rst ├── images └── sample_data_right_hip_angles.png ├── pykinematics ├── __init__.py ├── common.py ├── core.py ├── imu │ ├── __init__.py │ ├── lib │ │ ├── __init__.py │ │ ├── angles.py │ │ ├── calibration.py │ │ └── joints.py │ ├── optimize │ │ ├── __init__.py │ │ └── filters.py │ ├── orientation │ │ ├── __init__.py │ │ └── orientation.py │ └── utility.py ├── omc │ ├── __init__.py │ ├── angles.py │ ├── calibration.py │ ├── segmentFrames.py │ └── utility.py ├── tests │ ├── __init__.py │ ├── __main__.py │ ├── conftest.py │ ├── imu │ │ ├── conftest.py │ │ └── test_imu.py │ ├── omc │ │ ├── conftest.py │ │ └── test_omc.py │ ├── test_common.py │ └── test_imports.py └── version.py ├── scripts └── example_code.py └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | # lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | 106 | # PyCharm 107 | .idea 108 | .idea/* 109 | 110 | # test coverage 111 | test_coverage/ 112 | test_coverage/* 113 | -------------------------------------------------------------------------------- /.readthedocs.yml: -------------------------------------------------------------------------------- 1 | # .readthedocs.yml 2 | # Read the Docs configuration file 3 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 4 | 5 | # Required 6 | version: 2 7 | 8 | # Build documentation in the docs/ directory with Sphinx 9 | sphinx: 10 | builder: html 11 | configuration: docs/source/conf.py 12 | 13 | # Build documentation with MkDocs 14 | #mkdocs: 15 | # configuration: mkdocs.yml 16 | 17 | # Optionally build your docs in additional formats such as PDF and ePub 18 | formats: [] 19 | 20 | # Optionally set the version of Python and requirements required to build your docs 21 | python: 22 | version: 3.7 23 | install: 24 | - requirements: docs/requirements.txt 25 | - method: pip 26 | path: . 27 | - method: setuptools 28 | path: . 29 | system_packages: true 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pykinematics 2 | ``pykinematics`` is an open-source Python package for estimating hip kinematics using both novel Magnetic and Inertial 3 | Measurement Unit (MIMU) wearable sensors and existing Optical Motion Capture (OMC) algorithms. The novel MIMU algorithms 4 | have been validated against OMC, and include novel methods for estimating sensor-to-sensor relative orientation and 5 | sensor-to-segment alignment. 6 | 7 | ## Documentation 8 | 9 | Documentation including the below examples, and the API reference can be found at [pykinematics documentation](https://pykinematics.readthedocs.io/en/latest/) 10 | 11 | ## Requirements 12 | 13 | - Python >=3.6 14 | - Numpy 15 | - Scipy 16 | - h5py* 17 | 18 | pip should automatically collect any uninstalled dependencies. 19 | 20 | \* h5py is required to run the example code in `/scripts/example_code.py`, as the sample data 21 | provided (see *Example Usage*) is stored in the `.hdf` format. Pip will not catch and install 22 | `h5py` as it is not used by ``pykinematics``, and must be installed manually to run the example code. 23 | 24 | ```shell script 25 | pip install h5py 26 | ``` 27 | or if using Anaconda 28 | ```shell script 29 | conda install -c anaconda h5py 30 | ``` 31 | 32 | ## Installation 33 | 34 | ``pykinematics`` can be installed using pip: 35 | 36 | ```shell script 37 | pip install pykinematics 38 | ``` 39 | 40 | Alternatively, you can clone this repository and install from source. 41 | 42 | ``pykinematics`` can be uninstalled by running 43 | ```shell script 44 | pip uninstall pykinematics 45 | ``` 46 | 47 | ## Running tests 48 | Tests are implemented with [pytest](https://docs.pytest.org/en/latest/), and can be automatically run with: 49 | 50 | ```shell script 51 | pytest --pyargs pykinematics.tests 52 | ``` 53 | 54 | Optionally add `-v` to increase verbosity. 55 | 56 | If you don't want to run the integration tests (methods tests), use the following: 57 | ```shell script 58 | python -m pykinematics.tests --no-integration 59 | ``` 60 | 61 | If you want to see coverage, the following can be run (assuming [coverage](https://coverage.readthedocs.io/en/v4.5.x/) is installed): 62 | 63 | ```shell script 64 | coverage run -m pytest --pyargs pykinematics.tests 65 | # generate the report 66 | coverage report 67 | # generate a HTML report under ./build/index.html 68 | coverage html 69 | ``` 70 | 71 | ## Example Usage 72 | 73 | A full example script can be found in `/scripts/example_code.py`. This requires a sample 74 | data file, which can be downloaded from [Sample Data](https://www.uvm.edu/~rsmcginn/download/sample_data.h5) 75 | 76 | `example_code.py` contains a helper function to load the data into Python. 77 | Once the data is imported, the bulk of the processing is simple: 78 | 79 | ```python 80 | import pykinematics as pk 81 | 82 | static_calibration_data, star_calibration_data, walk_fast_data = 83 | 84 | # define some additional keyword arguments for optimizations and orientation estimation 85 | filt_vals = {'Angular acceleration': (2, 12)} 86 | 87 | ka_kwargs = {'opt_kwargs': {'method': 'trf', 'loss': 'arctan'}} 88 | jc_kwargs = dict(method='SAC', mask_input=True, min_samples=1500, opt_kwargs=dict(loss='arctan'), mask_data='gyr') 89 | orient_kwargs = dict(error_factor=5e-8, c=0.003, N=64, sigma_g=1e-3, sigma_a=6e-3) 90 | 91 | mimu_estimator = pk.ImuAngles(gravity_value=9.8404, filter_values=filt_vals, joint_center_kwargs=jc_kwargs, 92 | orientation_kwargs=orient_kwargs, knee_axis_kwargs=ka_kwargs) 93 | 94 | # calibrate the estimator based on Static and Star Calibration tasks 95 | mimu_estimator.calibrate(static_calibration_data, star_calibration_data) 96 | 97 | # compute the hip joint angles for the Fast Walking on a treadmill 98 | left_hip_angles, right_hip_angles = mimu_estimator.estimate(walk_fast_data, return_orientation=False) 99 | ``` 100 | 101 | Right hip angles from the sample data for walking fast: 102 | 103 | ![Sample right hip angles](https://github.com/M-SenseResearchGroup/pymotion/blob/master/images/sample_data_right_hip_angles.png "Sample right hip joint angles") 104 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=source 11 | set BUILDDIR=build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | numpydoc 3 | scipy 4 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | # import os 14 | # import sys 15 | # sys.path.insert(0, os.path.abspath('.')) 16 | import pykinematics as pk 17 | import sphinx_rtd_theme 18 | 19 | 20 | # -- Project information ----------------------------------------------------- 21 | 22 | project = 'pykinematics' 23 | copyright = '2019, Lukas Adamowicz' 24 | author = 'Lukas Adamowicz' 25 | 26 | # The full version, including alpha/beta/rc tags 27 | release = pk.__version__ 28 | # The short X.Y version 29 | version = pk.__version__[:3] 30 | 31 | 32 | # -- General configuration --------------------------------------------------- 33 | 34 | # Add any Sphinx extension module names here, as strings. They can be 35 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 36 | # ones. 37 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.intersphinx', 'sphinx.ext.autosummary', 'numpydoc', 'sphinx_rtd_theme'] 38 | 39 | # Add any paths that contain templates here, relative to this directory. 40 | templates_path = ['_templates'] 41 | 42 | autoclass_content = 'both' 43 | autosummary_generate = True 44 | # autosummary_imported_members = True 45 | 46 | # The suffix(es) of source filenames. 47 | # You can specify multiple suffix as a list of string: 48 | # 49 | # source_suffix = ['.rst', '.md'] 50 | source_suffix = '.rst' 51 | 52 | # The master toctree document. 53 | master_doc = 'index' 54 | 55 | # The language for content autogenerated by Sphinx. Refer to documentation 56 | # for a list of supported languages. 57 | # 58 | # This is also used if you do content translation via gettext catalogs. 59 | # Usually you set "language" from the command line for these cases. 60 | language = None 61 | 62 | # List of patterns, relative to source directory, that match files and 63 | # directories to ignore when looking for source files. 64 | # This pattern also affects html_static_path and html_extra_path. 65 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 66 | 67 | # The name of the Pygments (syntax highlighting) style to use. 68 | pygments_style = None 69 | 70 | # A lits of ignored prefixes for module index sorting 71 | modindex_common_prefix = ['pykinematics.'] 72 | 73 | 74 | # -- Options for HTML output ------------------------------------------------- 75 | 76 | # The theme to use for HTML and HTML Help pages. See the documentation for 77 | # a list of builtin themes. 78 | # 79 | html_theme = 'bizstyle' 80 | 81 | # Theme options are theme-specific and customize the look and feel of a theme 82 | # further. For a list of options available for each theme, see the 83 | # documentation. 84 | # 85 | html_theme_options = {'prev_next_buttons_location': 'both'} 86 | 87 | # Add any paths that contain custom static files (such as style sheets) here, 88 | # relative to this directory. They are copied after the builtin static files, 89 | # so a file named "default.css" will overwrite the builtin "default.css". 90 | html_static_path = ['_static'] 91 | 92 | # The name for this set of Sphinx documents. If None, it defaults to 93 | # " v documentation". 94 | html_title = 'pykinematics Documentation' 95 | 96 | # Custom sidebar templates, must be a dictionary that maps document names 97 | # to template names. 98 | # 99 | # The default sidebars (for documents that don't match any pattern) are 100 | # defined by theme itself. Builtin themes are using these templates by 101 | # default: ``['localtoc.html', 'relations.html', 'sourcelink.html', 102 | # 'searchbox.html']``. 103 | # 104 | html_sidebars = {'**': ['globaltoc.html', 'relations.html', 'searchbox.html']} 105 | 106 | 107 | # -- Options for HTMLHelp output --------------------------------------------- 108 | 109 | # Output file base name for HTML help builder. 110 | htmlhelp_basename = 'pykinematicsdoc' 111 | 112 | 113 | # -- Options for LaTeX output ------------------------------------------------ 114 | 115 | latex_elements = { 116 | # The paper size ('letterpaper' or 'a4paper'). 117 | # 118 | # 'papersize': 'letterpaper', 119 | 120 | # The font size ('10pt', '11pt' or '12pt'). 121 | # 122 | # 'pointsize': '10pt', 123 | 124 | # Additional stuff for the LaTeX preamble. 125 | # 126 | # 'preamble': '', 127 | 128 | # Latex figure (float) alignment 129 | # 130 | # 'figure_align': 'htbp', 131 | } 132 | 133 | # Grouping the document tree into LaTeX files. List of tuples 134 | # (source start file, target name, title, 135 | # author, documentclass [howto, manual, or own class]). 136 | latex_documents = [ 137 | (master_doc, 'pykinematics.tex', 'pykinematics Documentation', 138 | 'Lukas Adamowicz', 'manual'), 139 | ] 140 | 141 | 142 | # -- Options for manual page output ------------------------------------------ 143 | 144 | # One entry per manual page. List of tuples 145 | # (source start file, name, description, authors, manual section). 146 | man_pages = [ 147 | (master_doc, 'pykinematics', 'pykinematics Documentation', 148 | [author], 1) 149 | ] 150 | 151 | 152 | # -- Options for Texinfo output ---------------------------------------------- 153 | 154 | # Grouping the document tree into Texinfo files. List of tuples 155 | # (source start file, target name, title, author, 156 | # dir menu entry, description, category) 157 | texinfo_documents = [ 158 | (master_doc, 'pykinematics', 'pykinematics Documentation', 159 | author, 'pykinematics', 'Python based hip joint kinematics from wearable sensors and optical motion capture.', 160 | 'Miscellaneous'), 161 | ] 162 | 163 | 164 | # -- Options for Epub output ------------------------------------------------- 165 | 166 | # Bibliographic Dublin Core info. 167 | epub_title = project 168 | 169 | # The unique identifier of the text. This can be a ISBN number 170 | # or the project homepage. 171 | # 172 | # epub_identifier = '' 173 | 174 | # A unique identification for the text. 175 | # 176 | # epub_uid = '' 177 | 178 | # A list of files that should not be packed into the epub file. 179 | epub_exclude_files = ['search.html'] 180 | 181 | 182 | # -- Extension configuration ------------------------------------------------- 183 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. pykinematics documentation master file, created by 2 | sphinx-quickstart on Fri Oct 18 10:03:25 2019. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | pykinematics: 3D hip joint angle estimation 7 | =========================================== 8 | ``pykinematics`` is an open-source Python package for estimating hip kinematics using both novel Magnetic and Inertial 9 | Measurement Unit (MIMU) wearable sensors and existing Optical Motion Capture (OMC) algorithms. The novel MIMU algorithms 10 | have been validated against OMC, and include novel methods for estimating sensor-to-sensor relative orientation and 11 | sensor-to-segment alignment. 12 | 13 | Validation 14 | ---------- 15 | The novel MIMU algorithms in ``pykinematics`` were validated against OMC in a healthy human subject population. 16 | Detailed results and description of those novel algorithms can be found in [1]_. 17 | 18 | License 19 | ------- 20 | ``pykinematics`` is open source software distributed under a GNU GPL-3.0 license. 21 | 22 | Papers 23 | ------ 24 | 25 | .. [1] L. Adamowicz, R. Gurchiek, J. Ferri, A. Ursiny, N. Fiorentino, and R. McGinnis. "Novel Algorithms for Estimating Relative Orientation and Hip Joint Angles from Wearable Sensors." *Sensors*. Under-review. 26 | 27 | Contents 28 | -------- 29 | .. toctree:: 30 | :maxdepth: 2 31 | 32 | installation 33 | usage 34 | ref/index 35 | 36 | 37 | 38 | Indices and tables 39 | ================== 40 | 41 | * :ref:`genindex` 42 | * :ref:`modindex` 43 | * :ref:`search` 44 | -------------------------------------------------------------------------------- /docs/source/installation.rst: -------------------------------------------------------------------------------- 1 | .. pykinematics installation 2 | 3 | Installation & Requirements 4 | =========================== 5 | 6 | Installation 7 | ------------ 8 | ``pykinematics`` can be installed by running the following in the terminal: 9 | 10 | :: 11 | 12 | pip install git+https://github.com/M-SenseResearchGroup/pymotion 13 | 14 | 15 | Requirements 16 | ------------ 17 | These requirements will be collected if not already installed, and should require no input from the user. 18 | 19 | - Python >=3.6 20 | - NumPy 21 | - SciPy 22 | 23 | In order to run the example script using the sample data, `h5py` must be installed. 24 | 25 | :: 26 | 27 | pip install h5py 28 | 29 | or if using Anaconda 30 | 31 | :: 32 | 33 | conda install -c anaconda h5py 34 | -------------------------------------------------------------------------------- /docs/source/ref/core.rst: -------------------------------------------------------------------------------- 1 | .. pykinematics API reference: core 2 | .. currentmodule:: pykinematics 3 | 4 | pykinematics 5 | ============= 6 | 7 | .. toctree:: 8 | 9 | core/MimuAngles 10 | core/OmcAngles 11 | -------------------------------------------------------------------------------- /docs/source/ref/core/MimuAngles.rst: -------------------------------------------------------------------------------- 1 | pykinematics.MimuAngles 2 | ======================= 3 | 4 | .. currentmodule:: pykinematics 5 | 6 | .. autoclass:: MimuAngles 7 | :members: calibrate, estimate 8 | -------------------------------------------------------------------------------- /docs/source/ref/core/OmcAngles.rst: -------------------------------------------------------------------------------- 1 | pykinematics.OmcAngles 2 | ====================== 3 | 4 | .. currentmodule:: pykinematics 5 | 6 | .. autoclass:: OmcAngles 7 | 8 | 9 | .. automethod:: __init__ 10 | 11 | 12 | .. rubric:: Methods 13 | 14 | .. autosummary:: 15 | 16 | ~OmcAngles.__init__ 17 | ~OmcAngles.calibrate 18 | ~OmcAngles.estimate 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /docs/source/ref/imu.rst: -------------------------------------------------------------------------------- 1 | .. pykinematics API reference: imu 2 | .. currentmodule:: pykinematics 3 | 4 | pykinematics.imu 5 | ================ 6 | 7 | .. toctree:: 8 | 9 | imu/calibration 10 | imu/joints 11 | imu/orientation 12 | imu/angles 13 | imu/utility 14 | -------------------------------------------------------------------------------- /docs/source/ref/imu/angles.hip_from_frames.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.angles.hip_from_frames 2 | ======================================= 3 | 4 | .. currentmodule:: pykinematics.imu.angles 5 | 6 | .. autofunction:: hip_from_frames 7 | -------------------------------------------------------------------------------- /docs/source/ref/imu/angles.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.angles 2 | ======================= 3 | 4 | .. toctree:: 5 | 6 | angles.hip_from_frames 7 | -------------------------------------------------------------------------------- /docs/source/ref/imu/calibration.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.calibration 2 | =================================== 3 | 4 | .. toctree:: 5 | 6 | calibration.static 7 | -------------------------------------------------------------------------------- /docs/source/ref/imu/calibration.static.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.calibration.static 2 | =================================== 3 | 4 | .. currentmodule:: pykinematics.imu.calibration 5 | 6 | .. autofunction:: static 7 | -------------------------------------------------------------------------------- /docs/source/ref/imu/joints.Center.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.joints.Center 2 | ============================== 3 | 4 | .. currentmodule:: pykinematics.imu.joints 5 | 6 | .. autoclass:: Center 7 | :members: compute 8 | -------------------------------------------------------------------------------- /docs/source/ref/imu/joints.KneeAxis.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.joints.KneeAxis 2 | ================================ 3 | 4 | .. currentmodule:: pykinematics.imu.joints 5 | 6 | .. autoclass:: KneeAxis 7 | :members: compute 8 | -------------------------------------------------------------------------------- /docs/source/ref/imu/joints.correct_knee.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.joints.correct_knee 2 | ==================================== 3 | 4 | .. currentmodule:: pykinematics.imu.joints 5 | 6 | .. autofunction:: correct_knee 7 | -------------------------------------------------------------------------------- /docs/source/ref/imu/joints.fixed_axis.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.joints.fixed_axis 2 | ================================== 3 | 4 | .. currentmodule:: pykinematics.imu.joints 5 | 6 | .. autofunction:: fixed_axis 7 | -------------------------------------------------------------------------------- /docs/source/ref/imu/joints.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.joints 2 | ======================= 3 | 4 | .. toctree:: 5 | 6 | joints.Center 7 | joints.correct_knee 8 | joints.KneeAxis 9 | joints.fixed_axis 10 | -------------------------------------------------------------------------------- /docs/source/ref/imu/orientation.MadgwickAHRS.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.orientation.MadgwickAHRS 2 | ========================================= 3 | 4 | .. currentmodule:: pykinematics.imu.orientation 5 | 6 | .. autoclass:: MadgwickAHRS 7 | :members: update, updateIMU 8 | -------------------------------------------------------------------------------- /docs/source/ref/imu/orientation.OrientationComplementaryFilter.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.orientation.OrientationComplementaryFilter 2 | =========================================================== 3 | 4 | .. currentmodule:: pykinematics.imu.orientation 5 | 6 | .. autoclass:: OrientationComplementaryFilter 7 | :members: run 8 | -------------------------------------------------------------------------------- /docs/source/ref/imu/orientation.SSRO.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.orientation.SSRO 2 | ================================= 3 | 4 | .. currentmodule:: pykinematics.imu.orientation 5 | 6 | .. autoclass:: SSRO 7 | :members: run 8 | -------------------------------------------------------------------------------- /docs/source/ref/imu/orientation.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.orientation 2 | ============================ 3 | 4 | .. toctree:: 5 | 6 | orientation.MadgwickAHRS 7 | orientation.OrientationComplementaryFilter 8 | orientation.SSRO 9 | -------------------------------------------------------------------------------- /docs/source/ref/imu/pykinematics.imu.utility.calc_derivative.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility.calc\_derivative 2 | ========================================= 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autofunction:: calc_derivative -------------------------------------------------------------------------------- /docs/source/ref/imu/pykinematics.imu.utility.quat2matrix.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility.quat2matrix 2 | ==================================== 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autofunction:: quat2matrix -------------------------------------------------------------------------------- /docs/source/ref/imu/pykinematics.imu.utility.quat_conj.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility.quat\_conj 2 | =================================== 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autofunction:: quat_conj -------------------------------------------------------------------------------- /docs/source/ref/imu/pykinematics.imu.utility.quat_inv.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility.quat\_inv 2 | ================================== 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autofunction:: quat_inv -------------------------------------------------------------------------------- /docs/source/ref/imu/pykinematics.imu.utility.quat_mean.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility.quat\_mean 2 | =================================== 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autofunction:: quat_mean -------------------------------------------------------------------------------- /docs/source/ref/imu/pykinematics.imu.utility.quat_mult.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility.quat\_mult 2 | =================================== 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autofunction:: quat_mult -------------------------------------------------------------------------------- /docs/source/ref/imu/pykinematics.imu.utility.vec2quat.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility.vec2quat 2 | ================================= 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autofunction:: vec2quat -------------------------------------------------------------------------------- /docs/source/ref/imu/utility.rst: -------------------------------------------------------------------------------- 1 | pykinematics.imu.utility 2 | ======================== 3 | 4 | .. currentmodule:: pykinematics.imu.utility 5 | 6 | .. autosummary:: 7 | :toctree: 8 | 9 | calc_derivative 10 | quat_mult 11 | quat_conj 12 | quat_inv 13 | quat_mean 14 | vec2quat 15 | quat2matrix 16 | -------------------------------------------------------------------------------- /docs/source/ref/index.rst: -------------------------------------------------------------------------------- 1 | .. pykinematics API reference 2 | 3 | API Reference 4 | ============= 5 | 6 | .. currentmodule:: pykinematics 7 | 8 | .. toctree:: 9 | :maxdepth: 3 10 | 11 | core 12 | imu 13 | omc 14 | -------------------------------------------------------------------------------- /docs/source/ref/omc.rst: -------------------------------------------------------------------------------- 1 | .. pykinematics API reference: omc 2 | .. currentmodule:: pykinematics 3 | 4 | pykinematics.omc 5 | ================ 6 | 7 | .. toctree:: 8 | 9 | omc/calibration 10 | omc/segmentFrames 11 | omc/angles 12 | omc/utility 13 | -------------------------------------------------------------------------------- /docs/source/ref/omc/angles.hip.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.angles.hip 2 | =========================== 3 | 4 | .. currentmodule:: pykinematics.omc.angles 5 | 6 | .. autofunction:: hip 7 | -------------------------------------------------------------------------------- /docs/source/ref/omc/angles.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.angles 2 | ======================= 3 | 4 | .. toctree:: 5 | 6 | angles.hip 7 | -------------------------------------------------------------------------------- /docs/source/ref/omc/calibration.compute_hip_center.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.calibration.compute_hip_center 2 | =============================================== 3 | 4 | .. currentmodule:: pykinematics.omc.calibration 5 | 6 | .. autofunction:: compute_hip_center 7 | -------------------------------------------------------------------------------- /docs/source/ref/omc/calibration.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.calibration 2 | ============================ 3 | 4 | .. toctree:: 5 | 6 | calibration.compute_hip_center 7 | calibration.static 8 | -------------------------------------------------------------------------------- /docs/source/ref/omc/calibration.static.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.calibration.static 2 | =================================== 3 | 4 | .. currentmodule:: pykinematics.omc.calibration 5 | 6 | .. autofunction:: static 7 | -------------------------------------------------------------------------------- /docs/source/ref/omc/pykinematics.omc.utility.compute_pelvis_origin.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.utility.compute\_pelvis\_origin 2 | ================================================ 3 | 4 | .. currentmodule:: pykinematics.omc.utility 5 | 6 | .. autofunction:: compute_pelvis_origin -------------------------------------------------------------------------------- /docs/source/ref/omc/pykinematics.omc.utility.compute_shank_origin.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.utility.compute\_shank\_origin 2 | =============================================== 3 | 4 | .. currentmodule:: pykinematics.omc.utility 5 | 6 | .. autofunction:: compute_shank_origin -------------------------------------------------------------------------------- /docs/source/ref/omc/pykinematics.omc.utility.compute_thigh_origin.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.utility.compute\_thigh\_origin 2 | =============================================== 3 | 4 | .. currentmodule:: pykinematics.omc.utility 5 | 6 | .. autofunction:: compute_thigh_origin -------------------------------------------------------------------------------- /docs/source/ref/omc/pykinematics.omc.utility.create_cluster_frame.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.utility.create\_cluster\_frame 2 | =============================================== 3 | 4 | .. currentmodule:: pykinematics.omc.utility 5 | 6 | .. autofunction:: create_cluster_frame -------------------------------------------------------------------------------- /docs/source/ref/omc/pykinematics.omc.utility.default_marker_names.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.utility.default\_marker\_names 2 | =============================================== 3 | 4 | .. currentmodule:: pykinematics.omc.utility 5 | 6 | .. autofunction:: default_marker_names -------------------------------------------------------------------------------- /docs/source/ref/omc/segmentFrames.pelvis.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.segmentFrames.pelvis 2 | ===================================== 3 | 4 | .. currentmodule:: pykinematics.omc.segmentFrames 5 | 6 | .. autofunction:: pelvis 7 | -------------------------------------------------------------------------------- /docs/source/ref/omc/segmentFrames.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.segmentFrames 2 | ============================== 3 | 4 | .. toctree:: 5 | 6 | segmentFrames.pelvis 7 | segmentFrames.thigh 8 | -------------------------------------------------------------------------------- /docs/source/ref/omc/segmentFrames.thigh.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.segmentFrames.thigh 2 | ==================================== 3 | 4 | .. currentmodule:: pykinematics.omc.segmentFrames 5 | 6 | .. autofunction:: thigh 7 | -------------------------------------------------------------------------------- /docs/source/ref/omc/utility.rst: -------------------------------------------------------------------------------- 1 | pykinematics.omc.utility 2 | ======================== 3 | 4 | .. currentmodule:: pykinematics.omc.utility 5 | 6 | .. autosummary:: 7 | :toctree: 8 | 9 | default_marker_names 10 | create_cluster_frame 11 | compute_pelvis_origin 12 | compute_thigh_origin 13 | compute_shank_origin 14 | -------------------------------------------------------------------------------- /docs/source/usage.rst: -------------------------------------------------------------------------------- 1 | .. pykinematics usage 2 | 3 | ===================== 4 | Usage Examples 5 | ===================== 6 | 7 | Basic Use: MIMU Algorithms 8 | -------------------------- 9 | At the core of ``pykinematics`` for processing MIMU data is the *MimuAngles* class, which contains all the sub-methods required to 10 | calibrate and estimate hip joint angles bilaterally. Using the default parameters, estimating joint angles is then as simple as: 11 | 12 | .. code-block:: python 13 | 14 | import pykinematics as pk 15 | 16 | static_calibration_data, joint_center_task_data, trial_data = dummy_import_data() 17 | 18 | mimu_estimator = pk.MimuAngles() # initialize the estimator 19 | 20 | # calibrate based on a static standing trial and a trial for computing the joint center locations 21 | mimu_estimator.calibrate(static_calibration_data, joint_center_task_data) 22 | 23 | # estimate bilateral hip joint angles 24 | left_hip_angles, right_hip_angles = mimu_estimator.estimate(trial_data) 25 | 26 | Sample Data Example 27 | ------------------- 28 | An example script is provided under the github repository to run the 29 | sample data (`download sample data `_). 30 | 31 | .. code-block:: python 32 | 33 | import h5py 34 | import numpy as np 35 | import matplotlib.pyplot as plt 36 | 37 | import pykinematics as pk 38 | 39 | plt.style.use('ggplot') 40 | 41 | 42 | def import_sample_data(path_to_sample_data): 43 | # setup the dictionaries for storing the data 44 | # static calibration: shank data useful to create scaling factor for acceleration to match gravity 45 | stat_data = {'Lumbar': {}, 'Left thigh': {}, 'Right thigh': {}, 'Left shank': {}, 'Right shank': {}} 46 | # star calibration needs the shank data as well, for computation of the joint centers 47 | star_data = {'Lumbar': {}, 'Left thigh': {}, 'Right thigh': {}, 'Left shank': {}, 'Right shank': {}} 48 | walk_data = {'Lumbar': {}, 'Left thigh': {}, 'Right thigh': {}} 49 | 50 | tasks = ['Static Calibration', 'Star Calibration', 'Treadmill Walk Fast'] 51 | signals = [('Acceleration', 'Accelerometer'), ('Angular velocity', 'Gyroscope'), 52 | ('Magnetic field', 'Magnetometer'), ('Time', 'Time')] 53 | with h5py.File(path_to_sample_data, 'r') as file: 54 | for task, dat in zip(tasks, [stat_data, star_data, walk_data]): 55 | for loc in dat.keys(): 56 | for sigs in signals: 57 | dat[loc][sigs[0]] = np.zeros(file[task][loc.title()][sigs[1]].shape) 58 | file[task][loc.title()][sigs[1]].read_direct(dat[loc][sigs[0]]) 59 | for dat in [stat_data, star_data, walk_data]: 60 | for loc in dat.keys(): 61 | dat[loc]['Time'] = dat[loc]['Time'] / 1e6 # convert timestamps to seconds 62 | 63 | return stat_data, star_data, walk_data 64 | 65 | 66 | # import the raw sample data 67 | sample_data_path = 'W:\\Study Data\\Healthy Subjects\\sample_data.h5' 68 | static_calibration_data, star_calibration_data, walk_fast_data = import_sample_data(sample_data_path) 69 | 70 | # define some additional keyword arguments for optimizations and orientation estimation 71 | filt_vals = {'Angular acceleration': (2, 12)} 72 | 73 | ka_kwargs = {'opt_kwargs': {'method': 'trf', 'loss': 'arctan'}} 74 | jc_kwargs = dict(method='SAC', mask_input=True, min_samples=1500, opt_kwargs=dict(loss='arctan'), 75 | mask_data='gyr') 76 | orient_kwargs = dict(error_factor=5e-8, c=0.003, N=64, sigma_g=1e-3, sigma_a=6e-3) 77 | 78 | mimu_estimator = pk.MimuAngles(gravity_value=9.8404, filter_values=filt_vals, 79 | joint_center_kwargs=jc_kwargs, orientation_kwargs=orient_kwargs, 80 | knee_axis_kwargs=ka_kwargs) 81 | 82 | # calibrate the estimator based on Static and Star Calibration tasks 83 | mimu_estimator.calibrate(static_calibration_data, star_calibration_data) 84 | 85 | # compute the hip joint angles for the Fast Walking on a treadmill 86 | left_hip_angles, right_hip_angles = mimu_estimator.estimate(walk_fast_data, return_orientation=False) 87 | 88 | # PLOTTING 89 | fl, axl = plt.subplots(3, sharex=True) 90 | fr, axr = plt.subplots(3, sharex=True) 91 | label = [r'Flexion/Extension', 'Ad/Abduction', 'Internal/External Rotation'] 92 | for i in range(3): 93 | axl[i].plot(left_hip_angles[:, i]) 94 | axr[i].plot(right_hip_angles[:, i]) 95 | axl[i].set_title(label[i]) 96 | axr[i].set_title(label[i]) 97 | axl[i].set_ylabel('Angle [deg]') 98 | axr[i].set_ylabel('Angle [deg]') 99 | 100 | axl[2].set_xlabel('Sample') 101 | axr[2].set_xlabel('Sample') 102 | fl.suptitle('Left Hip Angles') 103 | fr.suptitle('Right Hip Angles') 104 | 105 | fl.tight_layout(rect=[0, 0.03, 1, 0.95]) 106 | fr.tight_layout(rect=[0, 0.03, 1, 0.95]) 107 | -------------------------------------------------------------------------------- /images/sample_data_right_hip_angles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/M-SenseResearchGroup/pykinematics/02d61da3cfc1e5562437523e35f4ff996f9a5bdd/images/sample_data_right_hip_angles.png -------------------------------------------------------------------------------- /pykinematics/__init__.py: -------------------------------------------------------------------------------- 1 | from pykinematics.version import __version__ 2 | from pykinematics import imu 3 | from pykinematics import omc 4 | from pykinematics.core import MimuAngles, OmcAngles 5 | -------------------------------------------------------------------------------- /pykinematics/common.py: -------------------------------------------------------------------------------- 1 | """ 2 | Methods that are common and needed accross both IMU and OMC methods 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | V0.1 - April 10, 2019 7 | """ 8 | from numpy import array, zeros, argmin, ceil, mean, std, sqrt, nanmean, nanstd 9 | 10 | 11 | def mov_avg(seq, window): 12 | """ 13 | Compute the centered moving average and standard deviation 14 | 15 | Parameters 16 | ---------- 17 | seq : numpy.ndarray 18 | Data to take the moving average and standard deviation on. 19 | window : int 20 | Window size for the moving average/standard deviation. 21 | 22 | Returns 23 | ------- 24 | m_mn : numpy.ndarray 25 | Moving average 26 | m_st : numpy.ndarray 27 | Moving standard deviation 28 | pad : int 29 | Padding at beginning of the moving average and standard deviation 30 | """ 31 | m_mn = zeros(seq.shape) 32 | m_st = zeros(seq.shape) 33 | 34 | if window < 2: 35 | window = 2 36 | 37 | pad = int(ceil(window / 2)) 38 | 39 | # compute first window stats 40 | if pad < seq.shape[0]: 41 | m_mn[pad] = mean(seq[:window], axis=0) 42 | m_st[pad] = std(seq[:window], axis=0, ddof=1)**2 # ddof of 1 indicates sample standard deviation, no population 43 | 44 | # compute moving mean and standard deviation 45 | for i in range(1, seq.shape[0] - window): 46 | diff_fl = seq[window + i - 1] - seq[i - 1] # difference in first and last elements of sliding window 47 | m_mn[pad + i] = m_mn[pad + i - 1] + diff_fl / window 48 | m_st[pad + i] = m_st[pad + i - 1] + (seq[window + i - 1] - m_mn[pad + i] + seq[i - 1] - 49 | m_mn[pad + i - 1]) * diff_fl / (window - 1) 50 | else: 51 | pad = 1 52 | m_mn[:] = nanmean(seq, axis=0) 53 | m_st[:] = nanstd(seq, axis=0, ddof=1)**2 54 | 55 | m_st = sqrt(abs(m_st)) # added absolute value to ensure that any round off error doesn't effect the results 56 | 57 | if window % 2 == 1: 58 | # m_mn = m_mn[:-1] 59 | # m_st = m_st[:-1] 60 | pass 61 | return m_mn, m_st, pad 62 | 63 | 64 | def find_most_still(data, window_size, return_index=False): 65 | """ 66 | Find the most still window of given data by using the minimum of the summed variances for each window. Returns 67 | the mean data during the most still window. 68 | 69 | Parameters 70 | ---------- 71 | data : tuple 72 | Tuple of data to use for determining the minimum summed variance for each window 73 | window_size : int 74 | Number of samples per window. 75 | return_index : bool 76 | Return the index of the lowest variance. 77 | 78 | Returns 79 | ------- 80 | still_data : tuple 81 | Tuple containing the mean data for each provided data stream for the most still window. 82 | ind : int, optional 83 | Index of the lowest variance window, excluding the padding at the start and end of the moving windows. 84 | """ 85 | var = [] 86 | means = tuple() 87 | still_data = tuple() 88 | for d in data: 89 | m_mn, m_st, pad = mov_avg(d, window_size) 90 | 91 | m_st = m_st ** 2 # square the standard deviation to get the variance 92 | 93 | means += (m_mn[pad:-pad], ) 94 | var.append(m_st[pad:-pad].sum(axis=1)) 95 | 96 | ind = argmin(array(var).sum(axis=0)) 97 | 98 | for mn in means: 99 | still_data += (mn[ind], ) 100 | 101 | if return_index: 102 | return still_data, ind 103 | else: 104 | return still_data 105 | 106 | 107 | -------------------------------------------------------------------------------- /pykinematics/core.py: -------------------------------------------------------------------------------- 1 | """ 2 | Core functions for computing joint angles from start to finish 3 | 4 | Lukas Adamowicz 5 | May 2019 6 | GNU GPL v3.0 7 | """ 8 | from numpy import mean, diff 9 | from scipy.signal import butter, filtfilt 10 | from warnings import warn 11 | 12 | from pykinematics import imu 13 | from pykinematics import omc 14 | 15 | 16 | class MimuAngles: 17 | def __init__(self, static_window=1.0, gravity_value=9.81, filter_values=None, angular_velocity_derivative_order=2, 18 | joint_center_kwargs=None, orientation_kwargs=None, correct_knee=True, knee_axis_kwargs=None, 19 | verbose=True): 20 | """ 21 | Compute angles from MIMU sensors, from initial raw data through joint angles. 22 | 23 | Parameters 24 | ---------- 25 | static_window : float, optional 26 | Window size in seconds for which to use in calibration during static standing. Default is 1s. 27 | gravity_value : float, optional 28 | Local gravitational acceleration. Default is 9.81m/s^2 29 | filter_values : {None, dict}, optional 30 | Filter values for the inertial and magnetic field data. Default is None, which uses the default settings. 31 | Providing a dictionary with any of the values modified (ie don't need to specify all 4) will changed the 32 | specific setting. Each entry is a length 2 tuple, containing first filter order, then cutoff frequency (Hz). 33 | Default settings and keys: 34 | - 'Acceleration': (2, 15) 35 | - 'Angular velocity': (2, 15) 36 | - 'Angular acceleration': (2, 15) 37 | - 'Magnetic field': (2, 15) 38 | angular_velocity_derivative_order : {2, 4}, optional 39 | Order for the calculation of the angular velocity derivative. Default is 2 for 2nd order. 40 | joint_center_kwargs : {None, dict}, optional 41 | Optional joint center computation key-word arguments, or None, for using the defaults. See 42 | :meth:`pykinematics.imu.joints.Center` for the possible arguments. Default is None. 43 | orientation_kwargs : {None, dict}, optional 44 | Optional sensor relative orientation key-word arguments, or NOne, for using the defaults. See 45 | :meth:`pykinematics.imu.orientation.SSRO` for the possible arguments. Default is None. 46 | correct_knee : bool, optional 47 | Correct the knee joint center location by shifting it along the rotation axis closer to the sensors. Default 48 | is True. 49 | knee_axis_kwargs : {None, dict}, optional 50 | Optional knee-axis computation key-word arguments. See :meth:`pykinematics.imu.joints.KneeAxis` for the 51 | possible arguments. Default is None 52 | verbose : bool, optional 53 | Print messages regarding the status of the estimation process. Default is True 54 | """ 55 | self.static_window = static_window 56 | self.grav_val = gravity_value 57 | 58 | # set the default filter values 59 | self.set_default_filter_values() 60 | if filter_values is not None: # see if any changes to be included 61 | if isinstance(filter_values, dict): # check that the format is followed 62 | for key in filter_values.keys(): # change the default values 63 | self.filt_vals[key] = filter_values[key] 64 | 65 | self.wd_ord = angular_velocity_derivative_order 66 | assert self.wd_ord == 2 or self.wd_ord == 4, 'The angular velocity derivative order must be either 2 or 4.' 67 | 68 | if joint_center_kwargs is not None: 69 | self.center_kwargs = joint_center_kwargs 70 | else: 71 | self.center_kwargs = {} 72 | if orientation_kwargs is not None: 73 | self.orient_kwargs = orientation_kwargs 74 | else: 75 | self.orient_kwargs = {} 76 | if knee_axis_kwargs is not None: 77 | self.knee_axis_kwargs = knee_axis_kwargs 78 | else: 79 | self.knee_axis_kwargs = {} 80 | 81 | self.correct_knee = correct_knee 82 | 83 | # calibrate attributes 84 | self.acc_scales = None 85 | self.pelvis_axis, self.l_thigh_axis, self.r_thigh_axis = None, None, None 86 | self.pelvis_AF, self.l_thigh_AF, self.r_thigh_AF = None, None, None 87 | 88 | self.verbose = verbose 89 | 90 | def estimate(self, task_data, return_orientation=False): 91 | """ 92 | Estimate joint angles from data during a trial of interest. 93 | 94 | Parameters 95 | ---------- 96 | task_data : dict 97 | Dictionary of data from a task to compute the hip joint angles for. Required keys/sensors are Lumbar, 98 | Left and Right thighs. See the *Notes* for *ImuAngles.calibrate*. 99 | return_orientation : bool, optional 100 | Return the estimates of the relative orientation. Default is False. 101 | 102 | Returns 103 | ------- 104 | left_hip_angles : numpy.ndarray 105 | (N, 3) array of hip angles for the left hip, in the order Flexion - Extension, Ad / Abduction, and 106 | Internal - External rotation. 107 | right_hip_angles : numpy.ndarray 108 | (N, 3) array of hip angles for the right hip, in the order Flexion - Extension, Ad / Abduction, and 109 | Internal - External rotation. 110 | R_Lthigh_lumbar : numpy.ndarray, optional 111 | Rotations from the left thigh sensor to the lumbar sensor for all time points in the provided data. 112 | Only returned if `return_orientation` is True. 113 | R_Rthigh_lumbar : numpy.ndarray, optional 114 | Rotations from the right thigh sensor to the lumbar sensor for all time points in the provided data. 115 | Only returned if `return_orientation` is True. 116 | q_Lthigh_lumbar : numpy.ndarray, optional 117 | Rotation quaternions from the left thigh sensor to the lumbar sensor for all time points in the provided data. 118 | Only returned if `return_orientation` is True. 119 | q_Rthigh_lumbar : numpy.ndarray, optional 120 | Rotation quaternions from the right thigh sensor to the lumbar sensor for all time points in the provided data. 121 | Only returned if `return_orientation` is True. 122 | """ 123 | # check to ensure that the data provided has the required sensors 124 | MimuAngles._check_required_sensors(task_data, 'trial') 125 | 126 | if self.verbose: 127 | print('-------------------------------------------------\nPreprocessing trial data...') 128 | 129 | # scale the acceleration data 130 | for sensor in task_data.keys(): 131 | task_data[sensor]['Acceleration'] *= self.acc_scales[sensor] 132 | 133 | # filter the data 134 | self._apply_filter_dict(task_data, comp_angular_accel=False) # don't need angular acceleration 135 | 136 | # compute the relative orientation 137 | if self.verbose: 138 | print('Computing sensor relative orientation...') 139 | srof = imu.orientation.SSRO(grav=self.grav_val, **self.orient_kwargs) 140 | q_lt_lb, R_lt_lb = MimuAngles._compute_orientation(srof, task_data['Lumbar'], task_data['Left thigh']) 141 | q_rt_lb, R_rt_lb = MimuAngles._compute_orientation(srof, task_data['Lumbar'], task_data['Right thigh']) 142 | 143 | # compute joint angles 144 | if self.verbose: 145 | print('Computing left and right hip joint angles...') 146 | l_hip_ang = imu.angles.hip_from_frames(self.pelvis_AF, self.l_thigh_AF, R_lt_lb, side='left') 147 | r_hip_ang = imu.angles.hip_from_frames(self.pelvis_AF, self.r_thigh_AF, R_rt_lb, side='right') 148 | 149 | if return_orientation: 150 | return l_hip_ang, r_hip_ang, R_lt_lb, R_rt_lb, q_lt_lb, q_rt_lb 151 | else: 152 | return l_hip_ang, r_hip_ang 153 | 154 | def calibrate(self, static_data, joint_center_data): 155 | """ 156 | Calibration by computing the sensor-to-segment alignment. 157 | 158 | Parameters 159 | ---------- 160 | static_data : dict 161 | Dictionary of sensor data from a static standing trial. Must contain at least Lumbar, Left thigh, and Right 162 | thigh keys. Left shank and Right shank are suggested in order to provide an acceleration scale to ensure 163 | that the measured gravity vectors are equivalent between sensors. See *Notes* for the structure of the 164 | dictionary. 165 | joint_center_data : dict 166 | Dictionary of sensor data from a task to be used for joint center computation. As such the task should have 167 | sufficient rotation about all possible axes (ie all 3 for the hip, 1 for the knee). Must contain Lumbar, 168 | thigh, and shank keys. See *Notes*. 169 | 170 | Attributes 171 | ---------- 172 | self.acc_scales : dict 173 | Dictionary of acceleration scales for each of the sensors in static_data, required to scale acceleration 174 | magnitude to that of local gravitational acceleration during static standing 175 | self.pelvis_axis : numpy.ndarray 176 | Pelvis fixed axis in the Lumbar sensor's reference frame. 177 | self.l_thigh_axis : numpy.ndarray 178 | Left thigh fixed axis in the left thigh's reference frame. 179 | self.r_thigh_axis : numpy.ndarray 180 | Right thigh fixed axis in the right thigh's reference frame. 181 | self.pelvis_AF : tuple 182 | Tuple of the x, y, and z anatomical axes for the pelvis, in the lumbar sensor frame. 183 | self.l_thigh_AF : tuple 184 | Tuple of the x, y, and z anatomical axes for the left thigh, in the left thigh's sensor frame. 185 | self.r_thigh_AF : tuple 186 | Tuple of the x, y, and z anatomical axes for the right thigh, in the right thigh's sensor frame. 187 | 188 | Notes 189 | ----- 190 | Data dictionary key-level schematic: 191 | 192 | - Lumbar 193 | - Time [s] 194 | - Acceleration [m/s^2] 195 | - Angular velocity [rad/s] 196 | - Magnetic field [not specified] 197 | - Left thigh 198 | - Time 199 | - Acceleration 200 | - Angular velocity 201 | - Magnetic field 202 | - Right thigh 203 | - Time 204 | - Acceleration 205 | - Angular velocity 206 | - Magnetic field 207 | - (Left shank) 208 | - Time 209 | - Acceleration 210 | - Angular velocity 211 | - Magnetic field 212 | - (Right shank) 213 | - Time 214 | - Acceleration 215 | - Angular velocity 216 | - Magnetic field 217 | """ 218 | # check to ensure that the data provided has the required sensors 219 | MimuAngles._check_required_sensors(static_data, 'static') 220 | MimuAngles._check_required_sensors(joint_center_data, 'joint center') 221 | 222 | if self.verbose: 223 | print('\n-----------------------------------------------------\n' 224 | 'Scaling acceleration and pre-processing the raw data') 225 | 226 | # get the acceleration scales 227 | self.acc_scales = dict() 228 | for sensor in static_data.keys(): 229 | self.acc_scales[sensor] = imu.calibration.get_acc_scale(static_data[sensor]['Acceleration'], 230 | gravity=self.grav_val) 231 | 232 | # scale the available data 233 | for sensor in self.acc_scales.keys(): 234 | static_data[sensor]['Acceleration'] *= self.acc_scales[sensor] 235 | 236 | if sensor in list(joint_center_data.keys()): 237 | joint_center_data[sensor]['Acceleration'] *= self.acc_scales[sensor] 238 | # issue a warning if any sensors are in the joint center data but not in the static data scales 239 | for sens in [sens for sens in joint_center_data.keys() if sens not in list(self.acc_scales.keys())]: 240 | warn(f'Sensor ({sens}) in joint center data has not been scaled due to no scale factor available from ' 241 | f'static data provided. Performance may suffer as a result.') 242 | 243 | # filter the static data 244 | self._apply_filter_dict(static_data, comp_angular_accel=False) 245 | # filter the joint center data 246 | self._apply_filter_dict(joint_center_data, comp_angular_accel=True) # need angular accel for this one 247 | 248 | if self.verbose: 249 | print('Computing joint centers...') 250 | # compute joint centers from the joint center data 251 | joint_center = imu.joints.Center(g=self.grav_val, **self.center_kwargs) 252 | 253 | # if the center method is "SAC", we need to compute the relative orientation first 254 | srof = imu.orientation.SSRO(grav=self.grav_val, **self.orient_kwargs) 255 | 256 | _, jcR_lt_lb = MimuAngles._compute_orientation(srof, joint_center_data['Lumbar'], 257 | joint_center_data['Left thigh']) 258 | _, jcR_rt_lb = MimuAngles._compute_orientation(srof, joint_center_data['Lumbar'], 259 | joint_center_data['Right thigh']) 260 | _, jcR_ls_lt = MimuAngles._compute_orientation(srof, joint_center_data['Left thigh'], 261 | joint_center_data['Left shank']) 262 | _, jcR_rs_rt = MimuAngles._compute_orientation(srof, joint_center_data['Right thigh'], 263 | joint_center_data['Right shank']) 264 | 265 | # compute the joint centers 266 | hip_l_lb, hip_l_t, hip_l_res = MimuAngles._compute_center(joint_center, joint_center_data['Lumbar'], 267 | joint_center_data['Left thigh'], jcR_lt_lb) 268 | hip_r_lb, hip_r_t, hip_r_res = MimuAngles._compute_center(joint_center, joint_center_data['Lumbar'], 269 | joint_center_data['Right thigh'], jcR_rt_lb) 270 | knee_l_t, knee_l_s, knee_l_res = MimuAngles._compute_center(joint_center, joint_center_data['Left thigh'], 271 | joint_center_data['Left shank'], jcR_ls_lt, 272 | self.correct_knee, self.knee_axis_kwargs) 273 | knee_r_t, knee_r_s, knee_r_res = MimuAngles._compute_center(joint_center, joint_center_data['Right thigh'], 274 | joint_center_data['Right shank'], jcR_rs_rt, 275 | self.correct_knee, self.knee_axis_kwargs) 276 | 277 | if self.verbose: 278 | print('------------------------------------------------------------') 279 | print( 280 | f'Left hip: Residual: {hip_l_res:0.3f}\nLumbar: ({hip_l_lb[0] * 100:0.2f}, {hip_l_lb[1] * 100:0.2f}, ' 281 | f'{hip_l_lb[2] * 100:0.2f})cm Left thigh: ({hip_l_t[0] * 100:0.2f}, {hip_l_t[1] * 100:0.2f}, ' 282 | f'{hip_l_t[2] * 100:0.2f})cm') 283 | print( 284 | f'Right hip: Residual: {hip_r_res:0.3f}\nLumbar: ({hip_r_lb[0] * 100:0.2f}, {hip_r_lb[1] * 100:0.2f}, ' 285 | f'{hip_r_lb[2] * 100:0.2f})cm Right thigh: ({hip_r_t[0] * 100:0.2f}, {hip_r_t[1] * 100:0.2f}, ' 286 | f'{hip_r_t[2] * 100:0.2f})cm') 287 | print( 288 | f'Left knee: Residual: {knee_l_res:0.3f}\nLeft thigh: ({knee_l_t[0] * 100:0.2f}, ' 289 | f'{knee_l_t[1] * 100:0.2f}, {knee_l_t[2] * 100:0.2f})cm Left shank: ({knee_l_s[0] * 100:0.2f}, ' 290 | f'{knee_l_s[1] * 100:0.2f}, {knee_l_s[2] * 100:0.2f})cm') 291 | print( 292 | f'Right knee: Residual: {knee_r_res:0.3f}\nRight thigh: ({knee_r_t[0] * 100:0.2f}, ' 293 | f'{knee_r_t[1] * 100:0.2f}, {knee_r_t[2] * 100:0.2f})cm Right shank: ({knee_r_s[0] * 100:0.2f}, ' 294 | f'{knee_r_s[1] * 100:0.2f}, {knee_r_s[2] * 100:0.2f})cm') 295 | print('------------------------------------------------------------') 296 | print('Computing fixed axes and creating anatomical reference frames') 297 | 298 | # compute the fixed axes for the thighs and pelvis 299 | self.pelvis_axis = imu.joints.fixed_axis(hip_l_lb, hip_r_lb, center_to_sensor=True) 300 | self.l_thigh_axis = imu.joints.fixed_axis(knee_l_t, hip_l_t, center_to_sensor=True) 301 | self.r_thigh_axis = imu.joints.fixed_axis(knee_r_t, hip_r_t, center_to_sensor=True) 302 | 303 | # compute the relative orientation between sensors during the static data 304 | q_lt_lb, _ = MimuAngles._compute_orientation(srof, static_data['Lumbar'], static_data['Left thigh']) 305 | q_rt_lb, _ = MimuAngles._compute_orientation(srof, static_data['Lumbar'], static_data['Right thigh']) 306 | 307 | # process the static calibration 308 | AF = imu.calibration.static(q_lt_lb, q_rt_lb, self.pelvis_axis, self.l_thigh_axis, self.r_thigh_axis, 309 | static_data['Lumbar']['Angular velocity'], 310 | static_data['Left thigh']['Angular velocity'], 311 | static_data['Right thigh']['Angular velocity'], static_data['Lumbar']['dt'], 312 | self.static_window) 313 | self.pelvis_AF, self.l_thigh_AF, self.r_thigh_AF = AF 314 | 315 | if self.verbose: 316 | print('Calibration complete\n') 317 | 318 | @staticmethod 319 | def _compute_center(jc, prox, dist, R_dist_prox, correct_knee=False, knee_axis_kwargs=None): 320 | """ 321 | Compute the joint center 322 | 323 | Parameters 324 | ---------- 325 | jc : pykinematics.imu.joints.Center 326 | Initialized joint center computation object 327 | prox : dict 328 | Dictionary, containing 'Acceleration', 'Angular velocity', and 'Angular acceleration' readings from a 329 | sensor. 330 | dist : dict 331 | Dictionary, containing 'Acceleration', 'Angular velocity', and 'Angular acceleration' readings from a 332 | sensor. 333 | R_dist_prox : numpy.ndarray 334 | (N, 3, 3) array of rotation matrices that align the distal sensor's frame with the proximal sensor's frame. 335 | correct_knee : bool, optional 336 | Whether or not to correct the knee joint. Should only be used for knee joint center computation. Default is 337 | False. 338 | knee_axis_kwargs : {None, dict}, optional 339 | Additional keyword arguments to be passed to the knee axis estimation, which is used for knee joint 340 | center correction. Default is None. 341 | 342 | Returns 343 | ------- 344 | prox_joint_center : numpy.ndarray 345 | Vector from the joint center to the proximal sensor. 346 | dist_joint_center : numpy.ndarray 347 | Vector from the joint center to the distal sensor. 348 | residual : float 349 | Residual value from the optimization process. 350 | """ 351 | # run the computation 352 | prox_jc, dist_jc, res = jc.compute(prox['Acceleration'], dist['Acceleration'], 353 | prox['Angular velocity'], dist['Angular velocity'], 354 | prox['Angular acceleration'], dist['Angular acceleration'], R_dist_prox) 355 | if correct_knee: 356 | imu.joints.correct_knee(prox['Angular velocity'], dist['Angular velocity'], prox_jc, dist_jc, 357 | R_dist_prox[0], knee_axis_kwargs) 358 | return prox_jc, dist_jc, res 359 | 360 | @staticmethod 361 | def _compute_orientation(sro, sensor1, sensor2): 362 | """ 363 | Run the orientation estimation filter. Rotation is provided from sensor 2 -> sensor 1 364 | 365 | Parameters 366 | ---------- 367 | sro : pykinematics.imu.orientation.SROFilter 368 | Object for estimating the sensor relative orientation 369 | sensor1 : dict 370 | Dictionary, containing 'Acceleration', 'Angular velocity', and 'Magnetic field' readings from a sensor. 371 | sensor2 : dict 372 | Dictionary, containing 'Acceleration', 'Angular velocity', and 'Magnetic field' readings from a sensor, 373 | which will be used to find the rotation from sensor2's frame to sensor1's frame 374 | 375 | Returns 376 | ------- 377 | q_21 : numpy.ndarray 378 | (N, 4) array quaternions representing the rotation to align sensor2's reference frame with that of sensor1's 379 | R_21 : numpy.ndarray 380 | (N, 3, 3) array of rotation matrices corresponding to the quaternions of 'q' 381 | """ 382 | x = sro.run(sensor1['Acceleration'], sensor2['Acceleration'], sensor1['Angular velocity'], 383 | sensor2['Angular velocity'], sensor1['Magnetic field'], sensor2['Magnetic field'], sensor1['dt']) 384 | # x is the length 10 vector of [gravity_1, gravity_2, q_21] 385 | R = imu.utility.quat2matrix(x[:, 6:]) # convert to a rotation matrix 386 | 387 | return x[:, 6:], R 388 | 389 | def _apply_filter_dict(self, data, comp_angular_accel=False): 390 | """ 391 | Apply a filter to a whole dictionary of sensor data, and calculate angular acceleration if necessary. 392 | 393 | Parameters 394 | ---------- 395 | data : dict 396 | Dictionary of data to apply the filter to 397 | comp_angular_accel : bool, optional 398 | Compute and filter angular acceleration. Default is False 399 | """ 400 | for sensor in data.keys(): 401 | # compute the sampling time difference, 1/f_sample 402 | data[sensor]['dt'] = mean(diff(data[sensor]['Time'])) 403 | # apply the specified filter to the acceleration 404 | data[sensor]['Acceleration'] = MimuAngles._apply_filter(data[sensor]['Acceleration'], data[sensor]['dt'], 405 | self.filt_vals['Acceleration'][0], 406 | self.filt_vals['Acceleration'][1]) 407 | # apply the specified filter to the angular velocity 408 | data[sensor]['Angular velocity'] = MimuAngles._apply_filter(data[sensor]['Angular velocity'], 409 | data[sensor]['dt'], 410 | self.filt_vals['Angular velocity'][0], 411 | self.filt_vals['Angular velocity'][1]) 412 | # apply the specified filter to the magnetic field reading 413 | data[sensor]['Magnetic field'] = MimuAngles._apply_filter(data[sensor]['Magnetic field'], 414 | data[sensor]['dt'], 415 | self.filt_vals['Magnetic field'][0], 416 | self.filt_vals['Magnetic field'][1]) 417 | 418 | if comp_angular_accel: 419 | data[sensor]['Angular acceleration'] = imu.utility.calc_derivative(data[sensor]['Angular velocity'], 420 | data[sensor]['dt'], 421 | order=self.wd_ord) 422 | data[sensor]['Angular acceleration'] = MimuAngles._apply_filter(data[sensor]['Angular acceleration'], 423 | data[sensor]['dt'], 424 | self.filt_vals['Angular acceleration'][0], 425 | self.filt_vals['Angular acceleration'][1]) 426 | 427 | @staticmethod 428 | def _apply_filter(x, dt, filt_order, filt_cutoff): 429 | """ 430 | Apply a filter to the data 431 | 432 | Parameters 433 | ---------- 434 | x : numpy.ndarray 435 | (N, M) array of data to be filtered. Will filter along the 0th axis (N) 436 | dt : float 437 | Sampling time difference between samples. 438 | filt_order : int 439 | Order of the filter to be applied 440 | filt_cutoff : float 441 | Cutoff frequency of the filter to be applied 442 | """ 443 | fc = butter(filt_order, filt_cutoff * 2 * dt) 444 | if x.ndim == 2: 445 | x = filtfilt(fc[0], fc[1], x, axis=0) 446 | elif x.ndim == 1: 447 | x = filtfilt(fc[0], fc[1], x) 448 | 449 | return x 450 | 451 | def set_default_filter_values(self): 452 | """ 453 | Set the filter values to the default: 454 | 455 | Angular velocity : (2, 15) 456 | Acceleration : (2, 15) 457 | Angular acceleration : (2, 15) 458 | Magnetic field : (2, 15) 459 | """ 460 | self.filt_vals = {'Angular velocity': (2, 15), 'Acceleration': (2, 15), 'Angular acceleration': (2, 15), 461 | 'Magnetic field': (2, 15)} 462 | 463 | @staticmethod 464 | def _check_required_sensors(data, data_use, bilateral=True): 465 | """ 466 | Check for the required sensors 467 | 468 | Parameters 469 | ---------- 470 | data : dict 471 | Dictionary of input data that will be used for estimating joint angles 472 | data_use : {'static', 'joint center', 'trial'} 473 | Use for the data. Either 'static', 'joint center', or 'trial' 474 | 475 | Raises 476 | ------ 477 | ValueError 478 | If the required sensors are missing from the dictionary 479 | """ 480 | if data_use == 'static': 481 | # required sensors : lumbar, left and right thigh 482 | req = ['Lumbar', 'Left thigh', 'Right thigh'] 483 | if not all([i in [j for j in data.keys()] for i in req]): 484 | raise ValueError(f'Static data does not have the required sensors. Ensure it has "Lumbar", ' 485 | f'"Left thigh", and "Right thigh" data.') 486 | elif data_use == 'joint center': 487 | # required sensors : lumbar, left and right thigh, left and right shank 488 | req = ['Lumbar', 'Left thigh', 'Right thigh', 'Left shank', 'Right shank'] 489 | if not all([i in [j for j in data.keys()] for i in req]): 490 | raise ValueError(f'Joint center computation data does not have the required sensors. Ensure it has ' 491 | f'"Lumbar", "Left thigh", "Right thigh", "Left shank", and "Right shank" data.') 492 | elif data_use == 'trial': 493 | # required sensors : lumbar, left and right thigh 494 | req = ['Lumbar', 'Left thigh', 'Right thigh'] 495 | if not all([i in [j for j in data.keys()] for i in req]): 496 | raise ValueError(f'Trial data does not have the required sensors. Ensure it has "Lumbar", ' 497 | f'"Left thigh", and "Right thigh" data.') 498 | 499 | 500 | class OmcAngles: 501 | def __init__(self, marker_names='default', window=1.0): 502 | """ 503 | Estimation of hip joint angles from optical motion capture marker trajectories. 504 | 505 | Parameters 506 | ---------- 507 | marker_names : {default, pykinematics.omc.MarkerNames}, optional 508 | List of marker names and what they are named in the data provided. Default value is `default`, which 509 | will use the default values, which can be seen through :meth:`pykinematics.omc.default_marker_names`. 510 | window : float, optional 511 | Window size in seconds of the duration to use to find the period of best stillness. Default is 1s. 512 | """ 513 | self.marker_names = marker_names 514 | self.window_t = window 515 | 516 | # initialize things used later 517 | self.pelvis = None 518 | self.left_thigh = None 519 | self.right_thigh = None 520 | 521 | def estimate(self, task_data, pelvis_cluster=False): 522 | """ 523 | Estimate hip joint angles from optical motion capture marker trajectories, following calibration. 524 | 525 | Parameters 526 | ---------- 527 | task_data : dict 528 | Dictionary of marker position data for a static standing trial, separated based on segment attached to. 529 | Requires at least Pelvis, Left thigh, and Right thigh. See *Notes* for the layout. 530 | pelvis_cluster : bool, optional 531 | Use the pelvis cluster to anatomical frame to create the anatomical frame for the trial, or create the 532 | pelvis anatomical frame from the marker trajectories (requires that the markers used to create the frame 533 | be present for the trial). 534 | 535 | Returns 536 | ------- 537 | left_hip_angles : numpy.ndarray 538 | Array of hip angles for the left hip, in the order Flexion - Extension, Ad / Abduction, and 539 | Internal - External rotation. 540 | right_hip_angles : numpy.ndarray 541 | Array of hip angles for the right hip, in the order Flexion - Extension, Ad / Abduction, and 542 | Internal - External rotation. 543 | """ 544 | # create the segment frames 545 | pelvis_af = omc.segmentFrames.pelvis(task_data['Pelvis'], use_cluster=pelvis_cluster, R_s_c=self.pelvis[2], 546 | marker_names=self.marker_names) 547 | lthigh_af = omc.segmentFrames.thigh(task_data['Left thigh'], 'left', use_cluster=True, R_s_c=self.left_thigh[2], 548 | marker_names=self.marker_names) 549 | rthigh_af = omc.segmentFrames.thigh(task_data['Right thigh'], 'right', use_cluster=True, 550 | R_s_c=self.right_thigh[2], 551 | marker_names=self.marker_names) 552 | 553 | # compute the joint angles 554 | l_hip_angles = omc.angles.hip(pelvis_af, lthigh_af, 'left') 555 | r_hip_angles = omc.angles.hip(pelvis_af, rthigh_af, 'right') 556 | 557 | return l_hip_angles, r_hip_angles 558 | 559 | def calibrate(self, static_data, joint_center_data, fs): 560 | """ 561 | Calibrate optical motion capture segments based on standard definitions of fixed and floating axes. 562 | 563 | Parameters 564 | ---------- 565 | static_data : dict 566 | Dictionary of marker position data for a static standing trial, separated based on segment attached to. 567 | Requires at least Pelvis, Left thigh, and Right thigh. See *Notes* for the layout. 568 | joint_center_data : dict 569 | Dictionary of marker position data for a static standing trial, separated based on segment attached to. 570 | Requires at least Pelvis, Left thigh, and Right thigh. See *Notes* for the layout. 571 | fs : float 572 | Sampling frequency in Hz. 573 | 574 | Attributes 575 | ---------- 576 | self.pelvis 577 | 3-tuple of pelvis anatomical frame, rotation from world to pelvis cluster frame, and constant rotation from 578 | segment frame to cluster frame. 579 | self.left_thigh : tuple 580 | 3-tuple of left thigh anatomical frame, rotation from world to the left thigh cluster frame, and constant 581 | rotation from segment frame to cluster frame. 582 | self.right_thigh : tuple 583 | 3-tuple of right thigh anatomical frame, rotation from world to the right thigh cluster frame, and constant 584 | rotation from segment frame to cluster frame. 585 | 586 | Notes 587 | ----- 588 | Data dictionary key-level schematic: 589 | 590 | - Pelvis 591 | - left_asis 592 | - right_asis 593 | - ... 594 | - Left thigh 595 | - left_lat_femoral_epicondyle 596 | - ... 597 | - Right thigh 598 | - right_lat_femoral_epicondyle 599 | - ... 600 | """ 601 | # TODO update omc.calibration.process_static to accept dictionaries. Or change the whole input 602 | # change to dumb format specified originally 603 | static_data_tuple = (static_data['Pelvis'], static_data['Left thigh'], static_data['Right thigh']) 604 | joint_center_data_tuple = (joint_center_data['Pelvis'], joint_center_data['Left thigh'], 605 | joint_center_data['Right thigh']) 606 | 607 | window = int(self.window_t * fs) 608 | # process the calibration data 609 | self.pelvis, self.left_thigh, self.right_thigh = omc.calibration.static(static_data_tuple, 610 | joint_center_data_tuple, 611 | window, self.marker_names) 612 | -------------------------------------------------------------------------------- /pykinematics/imu/__init__.py: -------------------------------------------------------------------------------- 1 | from pykinematics.imu.lib import angles 2 | from pykinematics.imu.lib import calibration 3 | from pykinematics.imu.lib import joints 4 | from pykinematics.imu import optimize 5 | from pykinematics.imu import orientation 6 | from pykinematics.imu import utility 7 | 8 | __all__ = ['optimize', 'orientation', 'angles', 'calibration', 'joints'] 9 | -------------------------------------------------------------------------------- /pykinematics/imu/lib/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ['angles', 'calibration', 'joints'] 2 | 3 | from pykinematics.imu.lib import angles 4 | from pykinematics.imu.lib import calibration 5 | from pykinematics.imu.lib import joints 6 | -------------------------------------------------------------------------------- /pykinematics/imu/lib/angles.py: -------------------------------------------------------------------------------- 1 | """ 2 | Methods for calculating joint angles using previously computed joint parameters and calibrated frames 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | 7 | V0.1 - March 8, 2019 8 | """ 9 | from numpy import cross, abs as nabs, arctan2 as atan2, sum, arccos, pi, stack 10 | from numpy.linalg import norm 11 | from scipy.integrate import cumtrapz 12 | 13 | 14 | __all__ = ['hip_from_frames'] 15 | 16 | 17 | def hip_from_frames(pelvis_AF, thigh_AF, R, side, zero_angles=False): 18 | """ 19 | Compute the hip joint angles from the segment fixed axes computed during the calibration. 20 | 21 | Parameters 22 | ---------- 23 | pelvis_AF : tuple 24 | Tuple of the x, y, and z axes of the pelvis anatomical frame. 25 | thigh_AF : tuple 26 | Tuple of the x, y, and z axes of the thigh anatomical frame. 27 | R : numpy.ndarray 28 | Nx3x3 array of rotation matrices from the thigh sensor frame to the pelvis sensor frame. 29 | side : {'left', 'right'} 30 | Side the angles are being computed for. 31 | zero_angles : bool, optional 32 | Remove any offset from zero at the start of the angles. Default is False. 33 | 34 | Returns 35 | ------- 36 | angles : numpy.ndarray 37 | Nx3 array of hip angles in degrees, with the first column being Flexion - Extension, 38 | second column being Ad / Abduction, and the third column being Internal - External Rotation. 39 | """ 40 | # get pelvis anatomical frame 41 | X = pelvis_AF[0] 42 | Y = pelvis_AF[1] 43 | Z = pelvis_AF[2] 44 | 45 | # get the thigh anatomical frame and rotate into pelvis frame 46 | x = R @ thigh_AF[0] 47 | y = R @ thigh_AF[1] 48 | z = R @ thigh_AF[2] 49 | 50 | # form the joint axes 51 | e1 = Z.copy() 52 | e3 = y.copy() 53 | 54 | e2 = cross(e3, e1) 55 | e2 /= norm(e2, axis=1, keepdims=True) 56 | 57 | # compute angles 58 | sgn = sum(cross(X, e2) * Z, axis=1) 59 | sgn /= nabs(sgn) 60 | fe = atan2(sgn * norm(cross(X, e2), axis=1), sum(X * e2, axis=1)) 61 | 62 | if side == 'right': 63 | # ad / abduction calculation 64 | aa = -pi / 2 + arccos(sum(e1 * e3, axis=1)) 65 | 66 | # internal - external rotation sign calculation 67 | sgn = sum(cross(x, e2) * -y, axis=1) 68 | elif side == 'left': 69 | aa = pi / 2 - arccos(sum(e1 * e3, axis=1)) 70 | sgn = sum(cross(x, e2) * y, axis=1) 71 | else: 72 | raise ValueError("side must be either 'left' or 'right'.") 73 | 74 | sgn /= nabs(sgn) 75 | ier = atan2(sgn * norm(cross(x, e2), axis=1), sum(x * e2, axis=1)) 76 | 77 | angles = stack((fe, aa, ier), axis=1) * 180 / pi 78 | if zero_angles: 79 | angles -= angles[0, :] 80 | 81 | return angles 82 | 83 | 84 | def hip_from_gyr(pelvis_w, thigh_w, pelvis_axis, thigh_axis, R, side): 85 | """ 86 | Compute hip angles by integrating the difference between the two sensors angular velocities about the axis 87 | of rotation. Typically not very noisy on higher frequencies, but on lower frequencies where it exhibits drift 88 | due to integration being used. 89 | 90 | Parameters 91 | ---------- 92 | pelvis_w : numpy.ndarray 93 | Nx3 array of angular velocity vectors in the pelvis sensor frame. 94 | thigh_w : numpy.ndarray 95 | Nx3 array of angular velocity vectors in the thigh sensor frame. 96 | pelvis_axis : numpy.ndarray 97 | Pelvis fixed axis. 98 | thigh_axis : numpy.ndarray 99 | Thigh fixed axis. 100 | R : numpy.ndarray 101 | Nx3x3 array of rotation matrices from the thigh sensor frame to the pelvis sensor frame. 102 | side : {'left', 'right'} 103 | Side angles are being computed for. 104 | 105 | Returns 106 | ------- 107 | angles : numpy.ndarray 108 | Nx3 array of joint angles for the trial. Columns are flexion - extension, ad / abduction, and 109 | internal - external rotation respectively. 110 | """ 111 | # get the joint rotation axes in the pelvis frame 112 | pelvis_e1 = pelvis_axis 113 | pelvis_e3 = R @ thigh_axis 114 | 115 | pelvis_e2 = cross(pelvis_e3, pelvis_e1) 116 | pelvis_e2 /= norm(pelvis_e2, axis=1, keepdims=True) 117 | 118 | # get the joint rotation axes in the thigh frame 119 | thigh_e1 = R.transpose([0, 2, 1]) @ pelvis_axis 120 | thigh_e3 = thigh_axis 121 | 122 | thigh_e2 = cross(thigh_e3, thigh_e1) 123 | thigh_e2 /= norm(thigh_e2, axis=1, keepdims=True) 124 | 125 | # compute the differences between angular rates around their respective rotation axes 126 | # TODO needs to be corrected still 127 | if side == 'left': 128 | fe_int = sum(pelvis_w * pelvis_e1, axis=1) - sum(thigh_w * thigh_e1, axis=1) 129 | aa_int = -sum(pelvis_w * pelvis_e2, axis=1) + sum(thigh_w * thigh_e2, axis=1) 130 | ier_int = sum(pelvis_w * pelvis_e3, axis=1) - sum(thigh_w * thigh_e3, axis=1) 131 | elif side == 'right': 132 | fe_int = sum(pelvis_w * pelvis_e1, axis=1) - sum(thigh_w * thigh_e1, axis=1) 133 | aa_int = sum(pelvis_w * pelvis_e2, axis=1) - sum(thigh_w * thigh_e2, axis=1) 134 | ier_int = -sum(pelvis_w * pelvis_e3, axis=1) + sum(thigh_w * thigh_e3, axis=1) 135 | else: 136 | raise ValueError("side must be 'left' or 'right'.") 137 | 138 | # integrate the differences in angular velocities to get angles. 139 | fe = cumtrapz(fe_int, dx=1 / 128, initial=0) 140 | aa = cumtrapz(aa_int, dx=1 / 128, initial=0) 141 | ier = cumtrapz(ier_int, dx=1 / 128, initial=0) 142 | 143 | angles = stack((fe, aa, ier), axis=1) * 180 / pi 144 | return angles 145 | -------------------------------------------------------------------------------- /pykinematics/imu/lib/calibration.py: -------------------------------------------------------------------------------- 1 | """ 2 | Methods for calibration of imu sensors for use in joint angle calculation 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | 7 | V0.1 - March 8, 2019 8 | """ 9 | from numpy import mean, ceil, cross 10 | from numpy.linalg import norm 11 | 12 | from pykinematics.imu import utility 13 | from pykinematics import common 14 | 15 | 16 | __all__ = ['get_acc_scale', 'static'] 17 | 18 | 19 | def get_acc_scale(acc, gravity=9.81): 20 | """ 21 | Get the acceleration scale factor so that the mean of the acceleration during the static trial is equal between 22 | sensors. 23 | 24 | Parameters 25 | ---------- 26 | acc : numpy.ndarray 27 | Nx3 array of accelerations during a static calibration trial. 28 | gravity : float, optional 29 | Value of local gravitational acceleration. Default is 9.81m/s^2 30 | 31 | Returns 32 | ------- 33 | scale : float 34 | Scale factor to multiply acceleration by 35 | """ 36 | mean_acc = mean(norm(acc, axis=1)) 37 | 38 | return gravity / mean_acc 39 | 40 | 41 | def static(lt_p_q, rt_p_q, pelvis_axis, l_thigh_axis, r_thigh_axis, pelvis_w, l_thigh_w, r_thigh_w, fs, window=1.0): 42 | """ 43 | Process a static standing data to create the anatomical axes for the pelvis and thighs. 44 | 45 | Parameters 46 | ---------- 47 | lt_p_q : numpy.ndarray 48 | Nx4 array of quaternions representing the rotation from the left thigh to the pelvis. 49 | rt_p_q : numpy.ndarray 50 | Nx4 array of quaternions representing the rotation from the right thigh to the pelvis. 51 | pelvis_axis : numpy.ndarray 52 | Pelvis fixed axis. 53 | l_thigh_axis : numpy.ndarray 54 | Left thigh fixed axis. 55 | r_thigh_axis : numpy.ndarray 56 | Right thigh fixed axis. 57 | pelvis_w : numpy.ndarray 58 | Nx3 array of angular velocities measured by the pelvis sensor. 59 | l_thigh_w : numpy.ndarray 60 | Nx3 array of angular velocities measured by the left thigh sensor. 61 | r_thigh_w : numpy.ndarray 62 | Nx3 array of angular velocities measured by the right thigh sensor. 63 | fs : float 64 | Sampling frequency of the sensors. 65 | window : float, optional 66 | Length of time for the most still period. Default is 1.0s. 67 | 68 | Returns 69 | ------- 70 | pelvis_AF : tuple 71 | Tuple of the x, y, and z axes of the pelvis anatomical frame. 72 | l_thigh_AF : tuple 73 | Tuple of the x, y, and z axes of the left thigh anatomical frame. 74 | r_thigh_AF : tuple 75 | Tuple of the x, y, and z axes of the right thigh anatomical frame. 76 | """ 77 | 78 | # find the most still period in the angular velocity data of the pelvis and thighs 79 | _, ind = common.find_most_still((pelvis_w, l_thigh_w, r_thigh_w), int(window * fs), return_index=True) 80 | 81 | pad = int(ceil(window * fs / 2)) 82 | 83 | # compute the rotations during the most still time 84 | if (ind - pad) < 0: 85 | l_q_mean = utility.quat_mean(lt_p_q[:ind + pad, :]) 86 | r_q_mean = utility.quat_mean(rt_p_q[:ind + pad, :]) 87 | else: 88 | l_q_mean = utility.quat_mean(lt_p_q[ind - pad:ind + pad, :]) 89 | r_q_mean = utility.quat_mean(rt_p_q[ind - pad:ind + pad, :]) 90 | 91 | lt_p_R = utility.quat2matrix(l_q_mean) 92 | rt_p_R = utility.quat2matrix(r_q_mean) 93 | 94 | # compute the left and right hip coordinate systems 95 | l_e1 = pelvis_axis 96 | l_e3 = lt_p_R @ l_thigh_axis 97 | l_e2 = cross(l_e3, l_e1) 98 | l_e2 /= norm(l_e2) 99 | 100 | r_e1 = pelvis_axis 101 | r_e3 = rt_p_R @ r_thigh_axis 102 | r_e2 = cross(r_e3, r_e1) 103 | r_e2 /= norm(r_e2) 104 | 105 | # form the pelvis anatomical axes 106 | p_z = pelvis_axis 107 | p_x = (r_e2 + l_e2) / 2 108 | p_y = cross(p_z, p_x) 109 | p_y /= norm(p_y) 110 | p_x = cross(p_y, p_z) 111 | p_x /= norm(p_x) 112 | 113 | # form the left thigh anatomical axes 114 | lt_x = lt_p_R.T @ l_e2 115 | lt_y = l_thigh_axis 116 | lt_z = cross(lt_x, lt_y) 117 | lt_z /= norm(lt_z) 118 | 119 | # form the right thigh anatomical axes 120 | rt_x = rt_p_R.T @ r_e2 121 | rt_y = r_thigh_axis 122 | rt_z = cross(rt_x, rt_y) 123 | rt_z /= norm(rt_z) 124 | 125 | # create the anatomical frames 126 | pelvis_AF = (p_x, p_y, p_z) 127 | l_thigh_AF = (lt_x, lt_y, lt_z) 128 | r_thigh_AF = (rt_x, rt_y, rt_z) 129 | 130 | return pelvis_AF, l_thigh_AF, r_thigh_AF 131 | -------------------------------------------------------------------------------- /pykinematics/imu/lib/joints.py: -------------------------------------------------------------------------------- 1 | """ 2 | Methods for calculating joint parameters, such as joint centers and axes. 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | 7 | V0.1 - March 8, 2019 8 | """ 9 | from numpy import array, zeros, logical_and, abs as nabs, concatenate, cross, std, sign, argmax, sqrt 10 | from numpy.linalg import lstsq, norm 11 | from scipy.optimize import least_squares 12 | 13 | 14 | __all__ = ['Center', 'KneeAxis', 'correct_knee', 'fixed_axis'] 15 | 16 | 17 | class Center: 18 | def __init__(self, g=9.81, method='SAC', mask_input=True, min_samples=1000, mask_data='acc', opt_kwargs=None): 19 | """ 20 | Estimation of joint centers using kinematic constraints. 21 | 22 | Parameters 23 | ---------- 24 | g : float, optional 25 | Local value of gravitational acceleration. Default is 9.81 m/s^2. 26 | method : {'SAC', 'SSFC'}, optional 27 | Method to use for the computation of the joint center. Default is SAC. See Crabolu et al. for more details. 28 | Method 'SAC' requires knowing the rotation between the sensors. 29 | mask_input : bool, optional 30 | Mask the input to only use the highest acceleration samples. Default is True 31 | min_samples : int, optional 32 | Minimum number of samples to use. Default is 1000. 33 | mask_data : {'acc', 'gyr'} 34 | Data to use for masking. Default is acceleration. 35 | opt_kwargs : dict, optional 36 | Optimization key-word arguments. SAC uses numpy.linalg.lstsq. 37 | SSFC and SSFCv use scipy.optimize.least_squares. 38 | 39 | References 40 | ---------- 41 | Crabolu et al. "In vivo estimation of the shoulder joint center of rotation using magneto-inertial sensors: 42 | MRI-based accuracy and repeatability assessment." *BioMedical Engineering Online*. 2017. 43 | """ 44 | self.g = g 45 | if method in ['SAC', 'SSFC']: 46 | self.method = method 47 | else: 48 | raise ValueError("Method must be either 'SAC' or 'SSFC'.") 49 | self.mask_input = mask_input 50 | self.min_samples = min_samples 51 | if mask_data in ['acc', 'gyr']: 52 | self.mask_data = mask_data 53 | else: 54 | raise ValueError("Masking data must be either 'acc' or 'gyr'.") 55 | if opt_kwargs is not None: 56 | self.opt_kwargs = opt_kwargs 57 | else: 58 | self.opt_kwargs = {} 59 | 60 | def compute(self, prox_a, dist_a, prox_w, dist_w, prox_wd, dist_wd, R_dist_prox): 61 | """ 62 | Perform the computation of the joint center to sensor vectors. 63 | 64 | Parameters 65 | ---------- 66 | prox_a : numpy.ndarray 67 | Nx3 array of accelerations measured by the joint proximal sensor. 68 | dist_a : numpy.ndarray 69 | Nx3 array of accelerations measured by the joint distal sensor. 70 | prox_w : numpy.ndarray 71 | Nx3 array of angular velocities measured by the joint proximal sensor. 72 | dist_w : numpy.ndarray 73 | Nx3 array of angular velocities measured by the joint distal sensor. 74 | prox_wd : numpy.ndarray 75 | Nx3 array of angular accelerations measured by the joint proximal sensor. 76 | dist_wd : numpy.ndarray 77 | Nx3 array of angular accelerations measured by the joint distal sensor. 78 | R_dist_prox : numpy.ndarray 79 | Nx3x3 array of rotations from the distal sensor frame to the proximal sensor frame. Ignored if method is 80 | 'SSFC'. 81 | 82 | Returns 83 | ------- 84 | prox_r : numpy.ndarray 85 | Joint center to proximal sensor vector. 86 | dist_r : numpy.ndarray 87 | Joint center to distal sensor vector. 88 | residual : float 89 | Residual value per sample used from the joint center optimization 90 | """ 91 | if self.method == 'SAC': 92 | if self.mask_input: 93 | if self.mask_data == 'acc': 94 | prox_data = norm(prox_a, axis=1) - self.g 95 | dist_data = norm(dist_a, axis=1) - self.g 96 | thresh = 1.0 97 | elif self.mask_data == 'gyr': 98 | prox_data = norm(prox_w, axis=1) 99 | dist_data = norm(dist_w, axis=1) 100 | thresh = 2.0 101 | 102 | mask = zeros(prox_data.shape, dtype=bool) 103 | 104 | while mask.sum() < self.min_samples: 105 | mask = logical_and(nabs(prox_data) > thresh, nabs(dist_data) > thresh) 106 | 107 | thresh -= 0.05 108 | if thresh < 0.09: 109 | raise ValueError('Not enough samples or samples with high motion in the trial provided. ' 110 | 'Use another trial') 111 | else: 112 | mask = zeros(prox_a.shape[0], dtype=bool) 113 | mask[:] = True 114 | 115 | # create the skew symmetric matrix products 116 | prox_K = array([[-prox_w[mask, 1] ** 2 - prox_w[mask, 2] ** 2, 117 | prox_w[mask, 0] * prox_w[mask, 1] - prox_wd[mask, 2], 118 | prox_wd[mask, 1] + prox_w[mask, 0] * prox_w[mask, 2]], 119 | [prox_wd[mask, 2] + prox_w[mask, 0] * prox_w[mask, 1], 120 | -prox_w[mask, 0] ** 2 - prox_w[mask, 2] ** 2, 121 | prox_w[mask, 1] * prox_w[mask, 2] - prox_wd[mask, 0]], 122 | [prox_w[mask, 0] * prox_w[mask, 2] - prox_wd[mask, 1], 123 | prox_wd[mask, 0] + prox_w[mask, 1] * prox_w[mask, 2], 124 | -prox_w[mask, 0] ** 2 - prox_w[mask, 1] ** 2]]).transpose([2, 0, 1]) 125 | 126 | dist_K = array([[-dist_w[mask, 1] ** 2 - dist_w[mask, 2] ** 2, 127 | dist_w[mask, 0] * dist_w[mask, 1] - dist_wd[mask, 2], 128 | dist_wd[mask, 1] + dist_w[mask, 0] * dist_w[mask, 2]], 129 | [dist_wd[mask, 2] + dist_w[mask, 0] * dist_w[mask, 1], 130 | -dist_w[mask, 0] ** 2 - dist_w[mask, 2] ** 2, 131 | dist_w[mask, 1] * dist_w[mask, 2] - dist_wd[mask, 0]], 132 | [dist_w[mask, 0] * dist_w[mask, 2] - dist_wd[mask, 1], 133 | dist_wd[mask, 0] + dist_w[mask, 1] * dist_w[mask, 2], 134 | -dist_w[mask, 0] ** 2 - dist_w[mask, 1] ** 2]]).transpose([2, 0, 1]) 135 | 136 | # create the oversized A and b matrices 137 | A = concatenate((prox_K, -R_dist_prox[mask] @ dist_K), axis=2).reshape((-1, 6)) 138 | b = (prox_a[mask].reshape((-1, 3, 1)) 139 | - R_dist_prox[mask] @ dist_a[mask].reshape((-1, 3, 1))).reshape((-1, 1)) 140 | 141 | # solve the linear least squares problem 142 | r, residual, _, _ = lstsq(A, b, rcond=None) 143 | r.resize((6,)) 144 | residual = residual[0] 145 | 146 | elif self.method == 'SSFC': 147 | r_init = zeros((6,)) 148 | 149 | if self.mask_input: 150 | if self.mask_data == 'acc': 151 | prox_data = norm(prox_a, axis=1) - self.g 152 | dist_data = norm(dist_a, axis=1) - self.g 153 | thresh = 1.0 154 | elif self.mask_data == 'gyr': 155 | prox_data = norm(prox_w, axis=1) 156 | dist_data = norm(dist_w, axis=1) 157 | thresh = 2.0 158 | 159 | mask = zeros(prox_data.shape, dtype=bool) 160 | 161 | while mask.sum() < self.min_samples: 162 | mask = logical_and(nabs(prox_data) > thresh, nabs(dist_data) > thresh) 163 | 164 | thresh -= 0.05 165 | if thresh < 0.09: 166 | raise ValueError('Not enough samples or samples with high motion in the trial provided. ' 167 | 'Use another trial') 168 | else: 169 | mask = zeros(prox_a.shape[0], dtype=bool) 170 | mask[:] = True 171 | 172 | # create the arguments to be passed to both the residual and jacobian calculation functions 173 | args = (prox_a[mask], dist_a[mask], prox_w[mask], dist_w[mask], prox_wd[mask], dist_wd[mask]) 174 | 175 | sol = least_squares(Center._compute_distance_residuals, r_init.flatten(), args=args, **self.opt_kwargs) 176 | r = sol.x 177 | residual = sol.cost 178 | 179 | return r[:3], r[3:], residual / mask.sum() 180 | 181 | @staticmethod 182 | def _compute_distance_residuals(r, a1, a2, w1, w2, wd1, wd2): 183 | """ 184 | Compute the residuals for the given joint center locations for proximal and distal inertial data 185 | 186 | Parameters 187 | ---------- 188 | r : numpy.ndarray 189 | 6x1 array of joint center locations. First three values are proximal location guess, last three values 190 | are distal location guess. 191 | a1 : numpy.ndarray 192 | Nx3 array of accelerations from the proximal sensor. 193 | a2 : numpy.ndarray 194 | Nx3 array of accelerations from the distal sensor. 195 | w1 : numpy.ndarray 196 | Nx3 array of angular velocities from the proximal sensor. 197 | w2 : numpy.ndarray 198 | Nx3 array of angular velocities from the distal sensor. 199 | wd1 : numpy.ndarray 200 | Nx3 array of angular accelerations from the proximal sensor. 201 | wd2 : numpy.ndarray 202 | Nx3 array of angular accelerations from the distal sensor. 203 | 204 | Returns 205 | ------- 206 | e : numpy.ndarray 207 | Nx1 array of residuals for the given joint center location guess. 208 | """ 209 | r1 = r[:3] 210 | r2 = r[3:] 211 | 212 | at1 = a1 - cross(w1, cross(w1, r1, axisb=0)) - cross(wd1, r1, axisb=0) 213 | at2 = a2 - cross(w2, cross(w2, r2, axisb=0)) - cross(wd2, r2, axisb=0) 214 | 215 | return norm(at1, axis=1) - norm(at2, axis=1) 216 | 217 | 218 | class KneeAxis: 219 | def __init__(self, mask_input=True, min_samples=1500, opt_kwargs=None): 220 | """ 221 | Estimation of the flexion/extension knee axis using kinematic constraints. 222 | 223 | Parameters 224 | ---------- 225 | mask_input : bool, optional 226 | Mask the input to only use samples with enough angular velocity to give a good estimate. Default is True. 227 | min_samples : int, optional 228 | Minimum number of samples to use in the optimization. Default is 1500. 229 | opt_kwargs : dict, optional 230 | Optimization key-word arguments. See scipy.optimize.least_squares. 231 | 232 | References 233 | ---------- 234 | Seel et al. "IMU-Based Joint Angle Measurement for Gait Analysis." *Sensors*. 2014 235 | Seel et al. "Joint axis and position estimation from inertial measurement data by exploiting kinematic 236 | constraints." *2012 IEEE International Conference on Control Applications*. 2012 237 | """ 238 | self.mask_input = mask_input 239 | self.min_samples = min_samples 240 | if opt_kwargs is not None: 241 | self.opt_kwargs = opt_kwargs 242 | else: 243 | self.opt_kwargs = {} 244 | 245 | def compute(self, thigh_w, shank_w): 246 | """ 247 | Compute the knee axis using the given angular velocities. 248 | 249 | Parameters 250 | ---------- 251 | thigh_w : numpy.ndarray 252 | Nx3 array of angular velocities measured by the thigh sensor. 253 | shank_w : numpy.ndarray 254 | Nx3 array of angular velocities measured by the shank sensor. 255 | 256 | Returns 257 | ------- 258 | thigh_j : numpy.ndarray 259 | Vector of the joint rotation axis in the thigh sensor frame. 260 | shank_j : numpy.ndarray 261 | Vector of the joint rotation axis in the shank sensor frame. 262 | """ 263 | j_init = (zeros(6) + 1) / sqrt(3) 264 | 265 | if self.mask_input: 266 | thigh_wn = norm(thigh_w, axis=1) 267 | shank_wn = norm(shank_w, axis=1) 268 | 269 | factor = 1.0 270 | mask = zeros(thigh_wn.size, dtype=bool) 271 | while mask.sum() < self.min_samples: 272 | mask = (thigh_wn > (factor * std(thigh_wn))) & (shank_wn > (factor * std(shank_wn))) 273 | factor -= 0.1 274 | if factor < 0.15: 275 | raise ValueError('Not enough samples to mask and still estimate the joint axis. Consider not ' 276 | 'masking, or use a trial with more samples') 277 | else: 278 | mask = array([True] * thigh_w.shape[0]) 279 | 280 | # arguments for the solver 281 | args = (thigh_w[mask], shank_w[mask]) 282 | # solve and normalize the result 283 | sol = least_squares(KneeAxis._compute_axis_residuals, j_init.flatten(), args=args, **self.opt_kwargs) 284 | thigh_j = sol.x[:3] / norm(sol.x[:3]) 285 | shank_j = sol.x[3:] / norm(sol.x[3:]) 286 | 287 | return thigh_j, shank_j 288 | 289 | @staticmethod 290 | def _compute_axis_residuals(j, thigh_w, shank_w): 291 | """ 292 | Compute the residuals using the estimate of the axis. 293 | 294 | Parameters 295 | ---------- 296 | j : numpy.ndarray 297 | Estimates of thigh and shank axis stacked into one vector 298 | thigh_w : numpy.ndarray 299 | Nx3 array of angular velocities measured by the thigh sensor. 300 | shank_w : numpy.ndarray 301 | Nx3 array of angular velocities measured by the shank sensor. 302 | 303 | Returns 304 | ------- 305 | e : numpy.ndarray 306 | N length array of residuals. 307 | """ 308 | j1 = j[:3] / norm(j[:3]) 309 | j2 = j[3:] / norm(j[3:]) 310 | 311 | wp1 = cross(thigh_w, j1) 312 | wp2 = cross(shank_w, j2) 313 | 314 | return norm(wp1, axis=1) - norm(wp2, axis=1) 315 | 316 | 317 | def correct_knee(thigh_w, shank_w, thigh_r, shank_r, R_shank_thigh, knee_axis_kwargs=None): 318 | """ 319 | Correct the knee position based on the computed knee axis. 320 | 321 | Parameters 322 | ---------- 323 | thigh_w : numpy.ndarray 324 | (N, 3) array of angular velocities measured by the thigh sensor. 325 | shank_w : numpy.ndarray 326 | (N, 3) array of angular velocities measured by the shank sensor. 327 | thigh_r : numpy.ndarray 328 | Initial knee joint center to thigh sensor vector. 329 | shank_r : numpy.ndarray 330 | Initial knee joint center to shank sensor vector. 331 | R_shank_thigh : numpy.ndarray 332 | (3, 3) Rotation matrix from shank to thigh sensors. 333 | knee_axis_kwargs : dict, optional 334 | Knee axis computation key-word arguments. See KneeAxis. 335 | 336 | Returns 337 | ------- 338 | thigh_r_corr : numpy.ndarray 339 | Corrected knee joint center to thigh sensor vector. 340 | shank_r_corr : numpy.ndarray 341 | Corrected knee joint center to shank sensor vector. 342 | """ 343 | if knee_axis_kwargs is None: 344 | knee_axis_kwargs = {} 345 | # compute the knee axis 346 | ka = KneeAxis(**knee_axis_kwargs) 347 | thigh_j, shank_j = ka.compute(thigh_w, shank_w) 348 | 349 | # check the sign of the major component of the axes when rotated into the same frame 350 | shank_j_thigh = R_shank_thigh @ shank_j 351 | if sign(shank_j_thigh[argmax(nabs(shank_j_thigh))]) != sign(thigh_j[argmax(nabs(thigh_j))]): 352 | shank_j *= -1 353 | 354 | # compute the corrections for the joint centers 355 | tmp = (sum(thigh_r * thigh_j) + sum(shank_r * shank_j)) / 2 356 | thigh_r_corr = thigh_r - thigh_j * tmp 357 | shank_r_corr = shank_r - shank_j * tmp 358 | 359 | return thigh_r_corr, shank_r_corr 360 | 361 | 362 | def fixed_axis(center1, center2, center_to_sensor=True): 363 | """ 364 | Compute the fixed axis of a segment based on computed joint centers. 365 | 366 | Parameters 367 | ---------- 368 | center1 : numpy.ndarray 369 | Location of the first joint center. This will be the "origin" of the axis. 370 | center2 : numpy.ndarray 371 | Location of the second joint center. This will be the "end" of the axis. 372 | center_to_sensor : bool, optional 373 | If the vectors provided are joint center to sensor (opposite is sensor to joint center). If True, then the 374 | axes are created in the opposite way as expected (eg right pointing pelvis axis would be left hip joint center 375 | minus the right hip joint center). Default is True. 376 | 377 | Returns 378 | ------- 379 | axis : numpy.ndarray 380 | Fixed axis based on joint centers, from center1 to center2. 381 | """ 382 | 383 | if center_to_sensor: 384 | axis = center1 - center2 385 | else: 386 | axis = center2 - center1 387 | 388 | # normalize 389 | axis /= norm(axis) 390 | return axis 391 | 392 | -------------------------------------------------------------------------------- /pykinematics/imu/optimize/__init__.py: -------------------------------------------------------------------------------- 1 | from pykinematics.imu.optimize.filters import * 2 | -------------------------------------------------------------------------------- /pykinematics/imu/optimize/filters.py: -------------------------------------------------------------------------------- 1 | """ 2 | Optimization filters 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | 7 | V0.1 - March 8, 2019 8 | """ 9 | from numpy import zeros, sqrt 10 | from numpy.linalg import inv, cholesky, pinv 11 | 12 | __all__ = ['UnscentedKalmanFilter', 'GaussNewton'] 13 | 14 | 15 | class UnscentedKalmanFilter: 16 | def __init__(self, x0, P0, F, H, Q, R): 17 | """ 18 | Parameters 19 | ---------- 20 | x0 : matrix_like, array_like 21 | Initial guess for state column vector (Nx1). 22 | P0 : matrix_like, array_like 23 | Initial guess for state covariance. Must be NxN size. 24 | F : callable 25 | Function to time-step state vector. x(t+dt) = F[x(t)] 26 | H : callable 27 | Function to estimate measured vector. z(t) = H[x(t);z(t)] 28 | Q : matrix_like,array_like,float,optional 29 | State covariance. Must be None, NxN matrix/array, or float. If None, 30 | defaults to 1.5*I where I is the NxN identity matrix 31 | R : matrix_like,array_like,float,str,optional 32 | Process covariance. Must be None, NxN matrix/array, or float. 33 | If None, defaults to 1. 34 | """ 35 | 36 | if x0.ndim == 1: 37 | self.x = x0.reshape((-1, 1)) 38 | else: 39 | self.x = x0 40 | 41 | self.P = P0 42 | self.F = F 43 | self.H = H 44 | self.Q = Q 45 | self.R = R 46 | 47 | def run(self, z, f_kwargs={}, h_kwargs={}, sigma_kwargs={}): 48 | """ 49 | Run Unscented Kalman filter for 1 step 50 | 51 | Parameters 52 | ---------- 53 | z : array_like 54 | mx1 array of measured data 55 | f_kwargs : dict,optional 56 | Any additional keyword arguments to be passed to state update function 57 | F. Optional, defaults to no arguments passed. 58 | h_kwargs : dict,optional 59 | Any additional keyword arguments to be passed to measured input estimation 60 | function H. Optional, defaults to no arguments passed. 61 | sigma_kwargs : dict, optional 62 | Sigma point computation key-word arguments. 63 | 64 | Returns 65 | ---------------- 66 | x : array_like 67 | Estimated state variable 68 | """ 69 | # make sure that z is 2D for proper broadcasting 70 | if z.ndim == 1: 71 | z.resize((z.size, 1)) 72 | 73 | # compute the sigma points from the state and state covariance 74 | Xi, W = UnscentedKalmanFilter.sigma_points(self.x, self.P, **sigma_kwargs) 75 | 76 | # update the sigma points 77 | fXi = self.F(Xi, **f_kwargs) 78 | 79 | # transform sigma points back to normal space 80 | xp, Px = UnscentedKalmanFilter.unscented_transform(fXi, W, self.Q) 81 | 82 | # get the measurement prediction from the sigma points 83 | hXi = self.H(fXi, **h_kwargs) 84 | 85 | # transform the measurement prediction to normal space 86 | zp, Pz = UnscentedKalmanFilter.unscented_transform(hXi, W, self.R) 87 | 88 | Pxz = zeros((self.x.shape[0], z.shape[0])) 89 | for k in range(2 * self.x.shape[0] + 1): 90 | Pxz += W[k] * (fXi[:, k].reshape((-1, 1)) - xp) @ (hXi[:, k].reshape((-1, 1)) - zp).T 91 | 92 | # calculate the Kalman gain 93 | K = Pxz @ inv(Pz) 94 | 95 | # get the best estimate of the state and state covariance 96 | self.x = xp + K @ (z - zp) 97 | self.P = Px - K @ Pz @ K.T 98 | 99 | return self.x 100 | 101 | @staticmethod 102 | def sigma_points(x, P, alpha=0.001, kappa=3.0, beta=2): 103 | """ 104 | Calculate Sigma Points 105 | 106 | Parameters 107 | ---------- 108 | x : array_like 109 | Vector to calculate sigma points for. 110 | P : array_like 111 | Covariance array used in sigma point calculation. 112 | alpha : float, optional 113 | Alpha value used in sigma point calculation. Defaults to 0.001. 114 | kappa : float, optional 115 | Kappa value used in sigma point calculation. Defaults to 0.0. 116 | beta : float, optional 117 | Beta value, based on the distribution of whats being estimated. 118 | 119 | Returns 120 | ------- 121 | xi : array 122 | Sigma points 123 | W : array 124 | Weights 125 | """ 126 | n = x.size 127 | Xi = zeros((n, 2 * n + 1)) 128 | W = zeros((2 * n + 1, 1)) 129 | 130 | # wiki also has different weights for the state and covariance 131 | # lambda in wiki, kappa in book 132 | y = alpha ** 2 * (n + kappa) - n 133 | 134 | Xi[:, 0] = x.flatten().copy() 135 | W[0] = y / (n + y) 136 | 137 | U = cholesky((n + y) * P) 138 | 139 | for i in range(n): 140 | Xi[:, i + 1] = x.flatten() + U[i, :] 141 | Xi[:, i + n + 1] = x.flatten() - U[i, :] 142 | 143 | W[1:] = 1 / (2 * (n + y)) 144 | 145 | return Xi, W 146 | 147 | @staticmethod 148 | def unscented_transform(Xi, W, noise_cov=0.0): 149 | """ 150 | Unscented transform 151 | 152 | Parameters 153 | ---------- 154 | Xi : array_like 155 | Sigma Points 156 | W : array_like 157 | Weights for sigma points 158 | noise_cov : float,optional 159 | Noise covariance. Optional, defaults to 0.0 160 | 161 | Returns 162 | ------- 163 | xm : array_like 164 | Mean of input sigma points from unscented transform 165 | xcov : array_like 166 | Covariance of input sigma points from unscented transform 167 | """ 168 | n, kmax = Xi.shape 169 | 170 | x = Xi @ W 171 | 172 | P = zeros((n, n)) 173 | for k in range(kmax): 174 | P += W[k] * (Xi[:, k].reshape((-1, 1)) - x) @ (Xi[:, k].reshape((-1, 1)) - x).T 175 | 176 | P += noise_cov 177 | 178 | return x, P 179 | 180 | 181 | class GaussNewton: 182 | def __init__(self, x0, tol=0.001, f_tol=0.001, max_iters=300, verbose=False): 183 | """ 184 | Gauss-Newton optimization function 185 | 186 | Parameters 187 | ---------- 188 | x0 : numpy.ndarray 189 | Mx1 array of initial guess for x 190 | tol : float, optional 191 | Tolerance for the change in x per iteration, as a percentage of x. Default is 0.001. 192 | f_tol : float, optional 193 | Tolerance for the change in the residual value per iteration, as a percentage of previous residual. 194 | Default is 0.001 195 | max_iters : int, optional 196 | Maximum number of iterations. Default is 300 197 | verbose : bool, optional 198 | Display convergence messages on the first 10 iterations, and then on iterations that are perfect squares. 199 | Default is False 200 | """ 201 | self.tol = tol 202 | self.ftol = f_tol 203 | self.max_iters = max_iters 204 | self.verbose = verbose 205 | 206 | if x0.ndim == 1: 207 | self.x = x0.reshape((-1, 1)).copy() 208 | else: 209 | self.x = x0.copy() 210 | 211 | def fit(self, f, J, f_args=(), f_kwargs={}, j_args=(), j_kwargs={}): 212 | """ 213 | Perform the optimization on x. 214 | 215 | Parameters 216 | ---------- 217 | f : callable 218 | Function to calculate the Nx1 array of residuals. First argument is x, the parameters being optimized. 219 | J : callable 220 | Function to calculate the NxM Jacobian matrix. First argument is x, the parameters being optimized. 221 | f_args : tuple, optional 222 | Additional arguments to pass to the residual calculation function. 223 | f_kwargs : dict, optional 224 | Additional key-word arguments to pass to the residual calculation function. 225 | j_args : tuple, optional 226 | Additional arguments to pass to the Jacobian calculation function. 227 | j_kwargs : dict, optional 228 | Additional key-word arguments to pass to the residual calculation function. 229 | 230 | Returns 231 | ------- 232 | x : numpy.ndarray 233 | Optimized result 234 | i : int 235 | Number of iterations to converge on solution 236 | fval : float 237 | Sum of squared errors at final solution 238 | """ 239 | 240 | # initialize stopping parameters 241 | dx = self.x.copy() 242 | pc = (abs(dx / self.x) > self.tol).any() # percent change in x is larger than tolerance 243 | fval = 1e12 244 | fc = 1 > self.ftol # percent change in sum of squared error is larger than error tolerance 245 | i =0 246 | 247 | while pc and fc and i < self.max_iters: 248 | e = f(self.x, *f_args, **f_kwargs) # calculate the residual vector 249 | j = J(self.x, *j_args, **j_kwargs) # calculate the jacobian 250 | 251 | j_inv = pinv(j) # calculate the moore-penrose inverse of J 252 | 253 | dx = j_inv @ e # calculate the update to x 254 | 255 | # calculate percent change in x parameter 256 | pc = (abs(dx / self.x) > self.tol).any() 257 | 258 | # update x 259 | self.x -= dx 260 | 261 | # calculate percent change in SSE, and SSE 262 | fc = abs(sum(e ** 2) - fval) / fval 263 | fval = sum(e ** 2) 264 | 265 | # print message if verbose is True 266 | if self.verbose: 267 | if sqrt(i) % 1 == 0 or i < 11: 268 | print(f'{i:5d} SSE per sample: {fval/e.shape[0]:10.3f}') 269 | 270 | # increment i 271 | i += 1 272 | 273 | return self.x, i, fval 274 | -------------------------------------------------------------------------------- /pykinematics/imu/orientation/__init__.py: -------------------------------------------------------------------------------- 1 | from pykinematics.imu.orientation.orientation import * 2 | -------------------------------------------------------------------------------- /pykinematics/imu/orientation/orientation.py: -------------------------------------------------------------------------------- 1 | """ 2 | Methods for calculating sensor absolute and relative orientations 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | 7 | V0.1 - March 8, 2019 8 | """ 9 | from numpy import array, zeros, cross, sqrt, abs as nabs, arccos, sin, mean, identity, sum, outer 10 | from numpy.linalg import norm, inv as np_inv 11 | 12 | from pykinematics.imu import utility 13 | from pykinematics.imu.optimize import UnscentedKalmanFilter 14 | 15 | 16 | __all__ = ['MadgwickAHRS', 'OrientationComplementaryFilter', 'SSRO'] 17 | 18 | 19 | class MadgwickAHRS: 20 | def __init__(self, sample_period=1/256, q_init=array([1, 0, 0, 0]), beta=0.041): 21 | """ 22 | Algorithm for estimating the orientation of an inertial sensor with or without a magnetometer. 23 | 24 | Parameters 25 | ---------- 26 | sample_period : float, optional 27 | Sampling period in seconds. Default is 1/256 (ie sampling frequency is 256Hz). 28 | q_init : numpy.ndarray, optional 29 | Initial quaternion estimate. Default is [1, 0, 0, 0]. 30 | beta : float, optional 31 | Beta value for the algorithm. Default is 1.0 32 | 33 | References 34 | ---------- 35 | S. Madgwick et al. "Estimation of IMU and MARG orientation using a gradient descent algorithm." *IEEE Intl. 36 | Conf. on Rehab. Robotics*. 2011. 37 | """ 38 | self.sample_period = sample_period 39 | self.q = q_init 40 | self.beta = beta 41 | 42 | def update(self, gyr, acc, mag): 43 | """ 44 | Perform update step. 45 | 46 | Parameters 47 | ---------- 48 | gyr : numpy.ndarray 49 | Angular velocity at time t. Units of rad/s. 50 | acc : numpy.ndarray 51 | Acceleration at time t. Units of g. 52 | mag : numpy.ndarray 53 | Magnetometer reading at time t. 54 | 55 | Returns 56 | ------- 57 | q : numpy.ndarray 58 | Quaternion estimate of orientation at time t. 59 | """ 60 | # short name for the quaternion 61 | q = self.q 62 | 63 | # normalize accelerometer measurement 64 | a = acc / norm(acc) 65 | 66 | # normalize magnetometer measurement 67 | h = mag / norm(mag) 68 | 69 | # reference direction of earth's magnetic field 70 | h_ref = utility.quat_mult(q, utility.quat_mult(array([0, h[0], h[1], h[2]]), utility.quat_conj(q))) 71 | b = array([0, norm(h_ref[1:3]), 0, h_ref[3]]) 72 | 73 | # Gradient Descent algorithm corrective step 74 | F = array([2 * (q[1] * q[3] - q[0] * q[2]) - a[0], 75 | 2 * (q[0] * q[1] + q[2] * q[3]) - a[1], 76 | 2 * (0.5 - q[1]**2 - q[2]**2) - a[2], 77 | 2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] * (q[1] * q[3] - q[0] * q[2]) - h[0], 78 | 2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] * (q[0] * q[1] + q[2] * q[3]) - h[1], 79 | 2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] * (0.5 - q[1]**2 - q[2]**2) - h[2]]) 80 | J = array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]], 81 | [2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]], 82 | [0, -4 * q[1], -4 * q[2], 0], 83 | [-2 * b[3] * q[2], 2 * b[3] * q[3], -4 * b[1] * q[2] - 2 * b[3] * q[0], -4 * b[1] * q[3] + 2 * b[3] * q[1]], 84 | [-2 * (b[1] * q[3] - b[3] * q[1]), 2 * (b[1] * q[2] + b[3] * q[0]), 2 * (b[1] * q[1] + b[3] * q[3]), -2 * (b[1] * q[0] - b[3] * q[2])], 85 | [2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1], 2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]]]) 86 | 87 | step = J.T @ F 88 | step /= norm(step) # normalize step magnitude 89 | 90 | # compute rate of change of quaternion 91 | qDot = 0.5 * utility.quat_mult(q, array([0, gyr[0], gyr[1], gyr[2]])) - self.beta * step 92 | 93 | # integrate to yeild quaternion 94 | self.q = q + qDot * self.sample_period 95 | self.q /= norm(self.q) 96 | 97 | return self.q 98 | 99 | def updateIMU(self, gyr, acc): 100 | """ 101 | Perform update step using only gyroscope and accelerometer measurements 102 | 103 | Parameters 104 | ---------- 105 | gyr : numpy.ndarray 106 | Angular velocity at time t. Units of rad/s. 107 | acc : numpy.ndarray 108 | Acceleration at time t. Units of g. 109 | 110 | Returns 111 | ------- 112 | q : numpy.ndarray 113 | Quaternion estimate of orientation at time t. 114 | """ 115 | a = acc / norm(acc) # normalize accelerometer magnitude 116 | q = self.q # short name 117 | # gradient descent algorithm corrective step 118 | F = array([2 * (q[1] * q[3] - q[0] * q[2]) - a[0], 119 | 2 * (q[0] * q[1] + q[2] * q[3]) - a[1], 120 | 2 * (0.5 - q[1]**2 - q[2]**2) - a[2]]) 121 | J = array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]], 122 | [2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]], 123 | [0, -4 * q[1], -4 * q[2], 0]]) 124 | step = J.T @ F 125 | step /= norm(step) # normalise step magnitude 126 | 127 | # compute rate of change quaternion 128 | q_dot = 0.5 * utility.quat_mult(q, array([0, gyr[0], gyr[1], gyr[2]])) - self.beta * step 129 | 130 | # integrate to yeild quaternion 131 | q = q + q_dot * self.sample_period 132 | self.q = q / norm(q) # normalise quaternion 133 | 134 | return self.q 135 | 136 | 137 | class OrientationComplementaryFilter: 138 | def __init__(self, g=9.81, alpha=0.1, beta=0.1, compute_bias=True, bias_alpha=0.1, acc_thresh=0.1, gyr_thresh=0.2, 139 | delta_gyr_thresh=0.01, adaptive_gain=True, gain_error_thresh=(0.1, 0.2), interp_thresh=0.9, 140 | acc_corr=True): 141 | """ 142 | Complementary filter for orientation estimation. 143 | 144 | Parameters 145 | ---------- 146 | g : float, optional 147 | Local value of gravitational acceleration. Default is 9.8 m/s^2. 148 | alpha : float, optional 149 | Acceleration complementary filter cut-off frequency. Default is 0.1. 150 | beta : float, optional 151 | Magnetic field complementary filter cut-off frequency. Default is 0.1. 152 | compute_bias : bool, optional 153 | Compute the bias of the angular velocity. Default is True. 154 | bias_alpha : float, optional 155 | Angular velocity bias gain. Default is 0.1 156 | acc_thresh : float, optional 157 | Threshold for determining still periods from acceleration for bias updating. Default is 0.1. 158 | gyr_thresh : float, optional 159 | Threshold for determining still periods from angular velocity for bias updating. Default is 0.2 160 | delta_gyr_thresh : float, optional 161 | Threshold for determining still periods from change since previous update of angular velocity for 162 | bias updating. Default is 0.01. 163 | adaptive_gain : bool, optional 164 | Use adaptive gain or not. Default is True. 165 | gain_error_thresh : array_like, optional 166 | Cutoff values for gain factor calculation. Default is (0.1, 0.2). 167 | interp_thresh : float, optional 168 | Below which value to use SLERP vs LERP. Default is 0.9 169 | acc_corr : bool, optional 170 | Correct acceleration by removing rotational acceleration. Default is True 171 | 172 | References 173 | ---------- 174 | Valenti et al. Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMU and MARGs. Sensors. 2015 175 | """ 176 | self.gravity = g 177 | 178 | self.alpha = alpha 179 | self.beta = beta 180 | 181 | self.comp_bias = compute_bias 182 | self.bias_alpha = bias_alpha 183 | 184 | self.acc_thresh = acc_thresh 185 | self.w_thresh = gyr_thresh 186 | self.delta_w_thresh = delta_gyr_thresh 187 | 188 | self.ada_gain = adaptive_gain 189 | self.ada_thresh1 = gain_error_thresh[0] 190 | self.ada_thresh2 = gain_error_thresh[1] 191 | # setup slope and intercept for adaptive gain factor calculation 192 | self.ada_m = 1 / (self.ada_thresh1 - self.ada_thresh2) 193 | self.ada_b = 1 - self.ada_m * self.ada_thresh1 194 | 195 | self.interp_thresh = interp_thresh 196 | 197 | self.acc_corr = acc_corr 198 | # initialize angular velocity bias 199 | self.w_bias = zeros(3) 200 | 201 | def run(self, acc, gyr, mag, dt, r): 202 | """ 203 | Run the orientation estimation over the data 204 | 205 | Parameters 206 | ---------- 207 | acc : numpy.ndarray 208 | Nx3 array of accelerations. 209 | gyr : numpy.ndarray 210 | Nx3 array of angular velocities. 211 | mag : numpy.ndarray 212 | Nx3 array of magnetic field readings. 213 | dt : float 214 | Sampling time. 215 | 216 | Attributes 217 | ---------- 218 | q_ : numpy.ndarray 219 | Nx4 array of orientation estimates from the global frame to the local frame. 220 | """ 221 | # store input values 222 | if self.acc_corr: 223 | corr = cross(gyr, cross(gyr, r)) 224 | self.a = acc - corr 225 | # store the acceleration norm since its useful 226 | self.a_mag = norm(self.a, axis=1) 227 | 228 | self.a /= norm(self.a, axis=1, keepdims=True) 229 | else: 230 | # store the acceleration norm since its useful 231 | self.a_mag = norm(acc, axis=1) 232 | self.a = acc / self.a_mag.reshape((-1, 1)) 233 | self.w = gyr.copy() 234 | self.h = mag / norm(mag, axis=1, keepdims=True) 235 | self.dt = dt 236 | 237 | # get the first estimate of q 238 | self.q = self._get_measurement(0) 239 | 240 | # get the number of elements 241 | n = self.a_mag.size 242 | 243 | # storage for quaternion estimate 244 | self.q_ = zeros((n, 4)) 245 | 246 | # run estimation procedure 247 | for i in range(n): 248 | # update the bias if doing so 249 | if self.comp_bias: 250 | self._update_biases(i) 251 | 252 | # get the prediction for the state 253 | q_pred = self._get_prediction(i) 254 | 255 | # get the acceleration based correction 256 | dq_acc = self._get_acc_correction(q_pred, i) 257 | 258 | # get the adaptive gain factor. Factor is 1 if not using an adaptive gain 259 | factor = self._get_adaptive_gain_factor(i) 260 | alpha = self.alpha * factor 261 | 262 | # interpolate the acceleration based correction 263 | dq_acc_int = self._get_scaled_quaternion(dq_acc, alpha) 264 | 265 | q_pred_acorr = utility.quat_mult(q_pred, dq_acc_int) 266 | 267 | # get the magnetometer based correction 268 | dq_mag = self._get_mag_correction(q_pred_acorr, i) 269 | 270 | # interpolate the magnetometer based correction 271 | dq_mag_int = self._get_scaled_quaternion(dq_mag, self.beta) 272 | 273 | # correct the prediction resulting in final estimate 274 | self.q = utility.quat_mult(q_pred_acorr, dq_mag_int) 275 | 276 | # normalize estimate 277 | self.q /= norm(self.q) 278 | 279 | # save the orientation estimate 280 | self.q_[i] = self.q 281 | 282 | def _get_measurement(self, ind): 283 | """ 284 | Get the initial measurement guess for the algorithm. 285 | """ 286 | if self.a[ind, 2] >= 0: 287 | b = sqrt(self.a[ind, 2] + 1) 288 | q_acc = array([b / sqrt(2), -self.a[ind, 1] / (sqrt(2) * b), self.a[ind, 0] / (sqrt(2) * b), 0]) 289 | else: 290 | b = sqrt(1 - self.a[ind, 2]) 291 | q_acc = array([-self.a[ind, 1] / (sqrt(2) * b), b / sqrt(2), 0, self.a[ind, 0] / (sqrt(2) * b)]) 292 | 293 | l = utility.quat2matrix(q_acc).T @ self.h[ind] 294 | Gamma = l[0]**2 + l[1]**2 295 | 296 | if l[0] >= 0: 297 | b = sqrt(Gamma + l[0] * sqrt(Gamma)) 298 | q_mag = array([b / sqrt(2 * Gamma), 0, 0, l[1] / (sqrt(2) * b)]) 299 | else: 300 | b = sqrt(Gamma - l[0] * sqrt(Gamma)) 301 | q_mag = array([l[1] / (sqrt(2) * b), 0, 0, b / sqrt(2 * Gamma)]) 302 | 303 | q_meas = utility.quat_mult(q_acc, q_mag) 304 | return q_meas 305 | 306 | def _get_state(self, ind): 307 | """ 308 | Check whether or not a time point is in steady state. 309 | 310 | Parameters 311 | ---------- 312 | ind : int 313 | Index of data to check 314 | 315 | Returns 316 | ------- 317 | steady_state : bool 318 | Whether or not the sensor is in a steady state. 319 | """ 320 | if abs(self.a_mag[ind] - self.gravity) > self.acc_thresh: 321 | return False 322 | elif (nabs(self.w[ind] - self.w[ind-1]) > self.delta_w_thresh).any(): 323 | return False 324 | elif (nabs(self.w[ind] - self.w_bias) > self.w_thresh).any(): 325 | return False 326 | else: 327 | return True 328 | 329 | def _update_biases(self, ind): 330 | """ 331 | Update the bias parameters if in steady state. 332 | 333 | Parameters 334 | ---------- 335 | ind : int 336 | Index of data to update from 337 | """ 338 | steady_state = self._get_state(ind) 339 | 340 | if steady_state: 341 | self.w_bias += self.bias_alpha * (self.w[ind] - self.w_bias) 342 | 343 | def _get_prediction(self, ind): 344 | """ 345 | Compute the predicted orientation estimate for the time point. 346 | 347 | Parameters 348 | ---------- 349 | ind : int 350 | Index of data to use. 351 | 352 | """ 353 | """ 354 | # construct the derivative calculation matrix 355 | Omega = zeros((4, 4)) 356 | Omega[0, 1:] = self.w[ind] - self.w_bias 357 | Omega[1:, 0] = -self.w[ind] - self.w_bias 358 | Omega[1:, 1:] = OrientationComplementaryFilter.skew_symmetric(self.w[ind] - self.w_bias) 359 | 360 | # compute the predicted orientation estimate 361 | q_pred = self.q + Omega @ self.q * self.dt 362 | """ 363 | wu = self.w[ind] - self.w_bias 364 | 365 | q_pred = self.q.copy() 366 | q_pred[0] += 0.5 * self.dt * (wu[0] * self.q[1] + wu[1] * self.q[2] + wu[2] * self.q[3]) 367 | q_pred[1] += 0.5 * self.dt * (-wu[0] * self.q[0] - wu[1] * self.q[3] + wu[2] * self.q[2]) 368 | q_pred[2] += 0.5 * self.dt * (wu[0] * self.q[3] - wu[1] * self.q[0] - wu[2] * self.q[1]) 369 | q_pred[3] += 0.5 * self.dt * (-wu[0] * self.q[2] + wu[1] * self.q[1] - wu[2] * self.q[0]) 370 | 371 | return q_pred 372 | 373 | def _get_acc_correction(self, q_pred, ind): 374 | """ 375 | Compute the acceleration based quaternion correction for the predicted value. 376 | 377 | Parameters 378 | ---------- 379 | ind : int 380 | Index to use for computation. 381 | 382 | Returns 383 | ------- 384 | dq : numpy.ndarray 385 | Quaternion correction. 386 | """ 387 | # compute the predicted gravity vector 388 | gp = utility.quat2matrix(utility.quat_inv(q_pred)) @ self.a[ind] 389 | gp /= norm(gp) 390 | # compute the correction quaternion 391 | # b = sqrt(gp[2] + 1) 392 | # dq = array([b / sqrt(2), -gp[1] / (sqrt(2) * b), gp[0] / (sqrt(2) * b), 0]) 393 | dq = zeros(4) 394 | dq[0] = sqrt((gp[2] + 1) * 0.5) 395 | dq[1] = -gp[1] / (2 * dq[0]) 396 | dq[2] = gp[0] / (2 * dq[0]) 397 | return dq / norm(dq) 398 | 399 | def _get_mag_correction(self, q_pred, ind): 400 | """ 401 | Compute the magnetometer based correction for the predicted orientation. 402 | 403 | Parameters 404 | ---------- 405 | q_pred : numpy.ndarray 406 | Predicted orientation estimate 407 | ind : int 408 | Index to compute correction at. 409 | 410 | Returns 411 | ------- 412 | dq : numpy.ndarray 413 | Quaternion correction 414 | """ 415 | # rotate the magnetic field vector into the current orientation estimate 416 | l = utility.quat2matrix(utility.quat_inv(q_pred)) @ self.h[ind] 417 | l /= norm(l) 418 | Gamma = l[0]**2 + l[1]**2 419 | 420 | # b = sqrt(Gamma + l[0] * sqrt(Gamma)) 421 | # dq = array([b / sqrt(2 * Gamma), 0, 0, l[1] / (sqrt(2) * b)]) 422 | 423 | beta = sqrt(Gamma + l[0] * sqrt(Gamma)) 424 | dq = zeros(4) 425 | dq[0] = beta / sqrt(2 * Gamma) 426 | dq[3] = l[1] / (sqrt(2) * beta) 427 | 428 | return dq / norm(dq) 429 | 430 | def _get_scaled_quaternion(self, q, gain, p=array([1, 0, 0, 0])): 431 | """ 432 | Scale the quaternion via linear interoplation or spherical linear interpolation. 433 | 434 | Parameters 435 | ---------- 436 | q : numpy.ndarray 437 | Quaternion to scale. 438 | p : numpy.ndarray 439 | Quaternion to scale towards. 440 | 441 | Returns 442 | ------- 443 | qs : numpy.ndarray 444 | Scaled quaternion. 445 | """ 446 | angle = arccos(sum(q * p)) 447 | if q[0] < self.interp_thresh: 448 | qs = sin((1 - gain) * angle) / sin(angle) * p + sin(gain * angle) / sin(angle) * q 449 | else: 450 | qs = (1 - gain) * p + gain * q 451 | 452 | return qs / norm(qs) 453 | 454 | def _get_adaptive_gain_factor(self, ind): 455 | """ 456 | Get the adaptive gain factor. 457 | 458 | Parameters 459 | ---------- 460 | ind : int 461 | Index to use for gain computation. 462 | 463 | Returns 464 | ------- 465 | factor : float 466 | Gain factor. 467 | """ 468 | factor = 1 469 | if not self.ada_gain: 470 | return factor 471 | else: 472 | err = abs(self.a_mag[ind] - self.gravity) / self.gravity 473 | if self.ada_thresh1 < err <= self.ada_thresh2: 474 | factor = self.ada_m * err + self.ada_b 475 | elif err > self.ada_thresh2: 476 | factor = 0 477 | return factor 478 | 479 | @staticmethod 480 | def _skew_symmetric(x): 481 | """ 482 | Get the skew symmetric representation of a vector. 483 | 484 | Parameters 485 | ---------- 486 | x : numpy.ndarray 487 | 1D vector in 3D space (ie 3 component vector) 488 | 489 | Returns 490 | ------- 491 | X : numpy.ndarray 492 | 3x3 skew symmetric matrix representation of x 493 | """ 494 | return array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) 495 | 496 | 497 | class SSRO: 498 | def __init__(self, c=0.04, N=16, error_factor=1e-15, sigma_g=1e-2, sigma_a=1e-1, grav=9.81, init_window=8): 499 | """ 500 | Sensor-to-Sensor Relative Orientation estimation algorithm. 501 | 502 | Parameters 503 | ---------- 504 | c : float, optional 505 | Value between 0 and 1 for the cutoff of the estimation of the gravity vector. Default is 0.04. 506 | N : int, optional 507 | Number of samples in the moving average measurement error estimation. Default is 16. 508 | error_factor : float, optional 509 | Error factor for the computation of the measurement error for the estimation of the rotation quaternion. 510 | Default is 1e-15, recommended is to be slightly less than the values in Q, which can be accomplished with 511 | values around dt**2 * sigma_g**2 * 0.1 512 | sigma_g : float, optional 513 | Gyroscope noise value. Default is 0.01 514 | sigma_a : float, optional 515 | Accelerometer noise value. Default is 0.1 516 | grav : float, optional 517 | Gravitational acceleration. Default is 9.81 m/s^2 518 | init_window : int, optional 519 | Number of samples to use for initialization of state and covariance. Default is 8. 520 | """ 521 | if not 0 <= c <= 1: 522 | raise ValueError('c must be between 0 and 1') 523 | self.c = c 524 | self.N = N 525 | self.err_factor = error_factor 526 | self.sigma_g = sigma_g 527 | self.sigma_a = sigma_a 528 | self.grav = grav 529 | self.init_window = init_window 530 | 531 | def run(self, s1_a, s2_a, s1_w, s2_w, s1_m, s2_m, dt): 532 | """ 533 | Run the SSRO algorithm on the data from two body-segment adjacent sensors. Rotation quaternion 534 | is from the second sensor to the first sensor. 535 | 536 | Parameters 537 | ---------- 538 | s1_w : numpy.ndarray 539 | Sensor 1 angular velocity in rad/s. 540 | s2_w : numpy.ndarray 541 | Sensor 2 angular velocity in rad/s. 542 | s1_a : numpy.ndarray 543 | Sensor 1 acceleration in m/s^2. 544 | s2_a : numpy.ndarray 545 | Sensor 2 acceleration in m/s^2. 546 | s1_m : numpy.ndarray 547 | Sensor 1 magnetometer readings. 548 | s2_m : numpy.ndarray 549 | Sensor 2 magnetometer readings. 550 | dt : float, optional 551 | Sample rate in seconds. 552 | 553 | Returns 554 | ------- 555 | q : numpy.ndarray 556 | Rotation quaternion containing the rotation from sensor 2's frame to that of sensor 1's 557 | """ 558 | # initialize values 559 | self.x = zeros(10) # state vector 560 | g1_init = mean(s1_a[:self.init_window, :], axis=0) 561 | g2_init = mean(s2_a[:self.init_window, :], axis=0) 562 | self.x[:3] = g1_init / norm(g1_init) 563 | self.x[3:6] = g2_init / norm(g2_init) 564 | 565 | m1_init = mean(s1_m[:self.init_window, :], axis=0) 566 | m2_init = mean(s2_m[:self.init_window, :], axis=0) 567 | mg1_init = m1_init - sum(m1_init * self.x[0:3]) * self.x[0:3] 568 | mg2_init = m2_init - sum(m2_init * self.x[3:6]) * self.x[3:6] 569 | 570 | q_g = utility.vec2quat(self.x[3:6], self.x[:3]) 571 | q_m = utility.vec2quat(utility.quat2matrix(q_g) @ mg2_init, mg1_init) 572 | self.x[6:] = utility.quat_mult(q_m, q_g) 573 | 574 | self.P = identity(10) * 0.01 575 | self.a1, self.a2 = zeros(3), zeros(3) 576 | self.a1_, self.a2_ = zeros(s1_w.shape), zeros(s1_w.shape) 577 | self.eps1, self.eps2 = zeros((3, 3)), zeros((3, 3)) 578 | 579 | # storage for the states 580 | self.x_ = zeros((s1_w.shape[0], 10)) 581 | 582 | # run the update step over all the data 583 | for i in range(s1_w.shape[0]): 584 | self.update(s1_a[i, :], s2_a[i, :], s1_w[i, :], s2_w[i, :], s1_m[i, :], s2_m[i, :], dt, i) 585 | self.x_[i] = self.x 586 | self.a1_[i] = self.a1 587 | self.a2_[i] = self.a2 588 | 589 | return self.x_ 590 | 591 | def update(self, y_a1, y_a2, y_w1, y_w2, y_m1, y_m2, dt, j): 592 | """ 593 | Update the state vector and covariance for the next time point. 594 | """ 595 | # short hand names for parts of the state vector 596 | g1 = self.x[:3] 597 | g2 = self.x[3:6] 598 | q_21 = self.x[6:] 599 | # compute the difference in angular velocities in the second sensor's frame 600 | R_21 = utility.quat2matrix(q_21) # rotation from sensor 2 to sensor 1 601 | y_w1_2 = R_21.T @ y_w1 # sensor 1 angular velocity in sensor 2 frame 602 | diff_y_w = y_w2 - y_w1_2 603 | 604 | # create the state transition matrix 605 | A = zeros((10, 10)) 606 | A[0:3, 0:3] = identity(3) - dt * SSRO._skew(y_w1) 607 | A[3:6, 3:6] = identity(3) - dt * SSRO._skew(y_w2) 608 | A[6:, 6:] = identity(4) - 0.5 * dt * SSRO._skew_quat(diff_y_w) 609 | 610 | # state transition covariance 611 | Q = zeros((10, 10)) 612 | Q[0:3, 0:3] = -dt**2 * SSRO._skew(g1) @ (self.sigma_g**2 * identity(3)) @ SSRO._skew(g1) 613 | Q[3:6, 3:6] = -dt**2 * SSRO._skew(g2) @ (self.sigma_g**2 * identity(3)) @ SSRO._skew(g2) 614 | Q[6:, 6:] = -dt**2 * SSRO._skew_quat(q_21) @ (self.sigma_g**2 * identity(4)) @ SSRO._skew_quat(q_21) 615 | 616 | # predicted state and covariance 617 | xhat = A @ self.x 618 | Phat = A @ self.P @ A.T + Q 619 | 620 | # create the measurements 621 | zt = zeros(10) 622 | zt[0:3] = y_a1 - self.c * self.a1 623 | zt[3:6] = y_a2 - self.c * self.a2 624 | # quaternion measurement 625 | q_g = utility.vec2quat(g2, g1) 626 | mg1 = y_m1 - sum(y_m1 * g1) * g1 627 | mg2 = y_m2 - sum(y_m2 * g2) * g2 628 | q_m = utility.vec2quat(utility.quat2matrix(q_g) @ mg2, mg1) 629 | 630 | zt[6:] = utility.quat_mult(q_m, q_g) 631 | 632 | # measurement estimation matrix 633 | H = zeros((10, 10)) 634 | H[:6, :6] = identity(6) * self.grav 635 | H[6:, 6:] = identity(4) 636 | 637 | # update estimates of the acceleration variance 638 | self.eps1 += self.c**2 / self.N * outer(self.a1, self.a1) 639 | self.eps1 -= self.c**2 / self.N * outer(self.a1_[j - self.N], self.a1_[j - self.N]) 640 | self.eps2 += self.c**2 / self.N * outer(self.a2, self.a2) 641 | self.eps2 -= self.c**2 / self.N * outer(self.a2_[j - self.N], self.a2_[j - self.N]) 642 | 643 | # measurement covariance - essentially how much we trust the measurement vector 644 | M = zeros((10, 10)) 645 | M[0:3, 0:3] = self.sigma_a**2 * identity(3) + self.eps1 646 | M[3:6, 3:6] = self.sigma_a**2 * identity(3) + self.eps2 647 | M[6:, 6:] = identity(4) * self.err_factor * (norm(self.a1) + norm(self.a2)) 648 | 649 | # kalman gain and state estimation 650 | K = Phat @ H.T @ np_inv(H @ Phat @ H.T + M) 651 | xbar = xhat + K @ (zt - H @ xhat) # not yet normalized 652 | self.P = (identity(10) - K @ H) @ Phat 653 | 654 | # normalize the state vector 655 | self.x[0:3] = xbar[0:3] / norm(xbar[0:3]) 656 | self.x[3:6] = xbar[3:6] / norm(xbar[3:6]) 657 | self.x[6:] = xbar[6:] / norm(xbar[6:]) 658 | 659 | # acceleration values 660 | self.a1 = y_a1 - self.x[0:3] * self.grav 661 | self.a2 = y_a2 - self.x[3:6] * self.grav 662 | 663 | @staticmethod 664 | def _skew(v): 665 | return array([[0, -v[2], v[1]], 666 | [v[2], 0, -v[0]], 667 | [-v[1], v[0], 0]]) 668 | 669 | @staticmethod 670 | def _skew_quat(v): 671 | if v.size == 3: 672 | Q = array([[0, v[0], v[1], v[2]], 673 | [-v[0], 0, -v[2], v[1]], 674 | [-v[1], v[2], 0, -v[0]], 675 | [-v[2], -v[1], v[0], 0]]) 676 | elif v.size == 4: 677 | Q = array([[v[0], v[1], v[2], v[3]], 678 | [-v[1], v[0], -v[3], v[2]], 679 | [-v[2], v[3], v[0], -v[1]], 680 | [-v[3], -v[2], v[1], v[0]]]) 681 | else: 682 | raise ValueError("Input can only have 3 or 4 elements") 683 | return Q 684 | 685 | -------------------------------------------------------------------------------- /pykinematics/imu/utility.py: -------------------------------------------------------------------------------- 1 | """ 2 | General utility methods used in the calculation of joint angles from inertial sensors 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | 7 | V0.1 - March 8, 2019 8 | """ 9 | from numpy import array, zeros, argsort, dot, arccos, cross, cos, sin, real, allclose 10 | from numpy.linalg import norm, eig 11 | 12 | 13 | def calc_derivative(x, dt, order=2): 14 | """ 15 | Calculate the 2nd or 4th order derivative of a sequence. 16 | 17 | Parameters 18 | ---------- 19 | x : numpy.ndarray 20 | 1 or 2D array to take the derivative of. 21 | dt : float 22 | Time difference for all points. This cannot handle varying time differences. 23 | order : {4, 2}, optional 24 | Order of the derivative to calculate. Default is 2nd order. 25 | 26 | Returns 27 | ------- 28 | dx : numpy.ndarray 29 | Derivative of x. 30 | """ 31 | dx = zeros(x.shape) 32 | 33 | if order == 4: 34 | dx[2:-2] = (x[:-4] - 8 * x[1:-3] + 8 * x[3:-1] + x[4:]) / (12 * dt) 35 | 36 | # edges 37 | dx[:2] = (-25 * x[:2] + 48 * x[1:3] - 36 * x[2:4] + 16 * x[3:5] - 3 * x[4:6]) / (12 * dt) 38 | dx[-2:] = (25 * x[-2:] - 48 * x[-3:-1] + 36 * x[-4:-2] - 16 * x[-5:-3] + 3 * x[-6:-4]) / (12 * dt) 39 | elif order == 2: 40 | dx[1:-1] = (x[2:] - x[:-2]) / (2 * dt) 41 | 42 | # edges 43 | dx[0] = (-3 * x[0] + 4 * x[1] - x[2]) / (2 * dt) 44 | dx[-1] = (3 * x[-1] - 4 * x[-2] + x[-3]) / (2 * dt) 45 | 46 | return dx 47 | 48 | 49 | def quat_mult(q1, q2): 50 | """ 51 | Multiply quaternions 52 | 53 | Parameters 54 | ---------- 55 | q1 : numpy.ndarray 56 | 1x4 array representing a quaternion 57 | q2 : numpy.ndarray 58 | 1x4 array representing a quaternion 59 | 60 | Returns 61 | ------- 62 | q : numpy.ndarray 63 | 1x4 quaternion product of q1*q2 64 | """ 65 | if q1.shape != (1, 4) and q1.shape != (4, 1) and q1.shape != (4,): 66 | raise ValueError('Quaternions contain 4 dimensions, q1 has more or less than 4 elements') 67 | if q2.shape != (1, 4) and q2.shape != (4, 1) and q2.shape != (4,): 68 | raise ValueError('Quaternions contain 4 dimensions, q2 has more or less than 4 elements') 69 | if q1.shape == (4, 1): 70 | q1 = q1.T 71 | 72 | Q = array([[q2[0], q2[1], q2[2], q2[3]], 73 | [-q2[1], q2[0], -q2[3], q2[2]], 74 | [-q2[2], q2[3], q2[0], -q2[1]], 75 | [-q2[3], -q2[2], q2[1], q2[0]]]) 76 | 77 | return q1 @ Q 78 | 79 | 80 | def quat_conj(q): 81 | """ 82 | Compute the conjugate of a quaternion 83 | 84 | Parameters 85 | ---------- 86 | q : numpy.ndarray 87 | Nx4 array of N quaternions to compute the conjugate of. 88 | 89 | Returns 90 | ------- 91 | q_conj : numpy.ndarray 92 | Nx4 array of N quaternion conjugats of q. 93 | """ 94 | return q * array([1, -1, -1, -1]) 95 | 96 | 97 | def quat_inv(q): 98 | """ 99 | Invert a quaternion 100 | 101 | Parameters 102 | ---------- 103 | q : numpy.ndarray 104 | 1x4 array representing a quaternion 105 | 106 | Returns 107 | ------- 108 | q_inv : numpy.ndarray 109 | 1x4 array representing the inverse of q 110 | """ 111 | # TODO change this, then remove the test that it doesn't work on arrays of quaternions 112 | if q.size != 4: 113 | raise ValueError('Not currently implemented for arrays of quaternions.') 114 | q_conj = q * array([1, -1, -1, -1]) 115 | return q_conj / sum(q ** 2) 116 | 117 | 118 | def quat2matrix(q): 119 | """ 120 | Transform quaternion to rotation matrix 121 | 122 | Parameters 123 | ---------- 124 | q : numpy.ndarray 125 | Quaternion 126 | 127 | Returns 128 | ------- 129 | R : numpy.ndarray 130 | Rotation matrix 131 | """ 132 | if q.ndim == 1: 133 | s = norm(q) 134 | R = array([[1 - 2 * s * (q[2] ** 2 + q[3] ** 2), 2 * s * (q[1] * q[2] - q[3] * q[0]), 135 | 2 * s * (q[1] * q[3] + q[2] * q[0])], 136 | [2 * s * (q[1] * q[2] + q[3] * q[0]), 1 - 2 * s * (q[1] ** 2 + q[3] ** 2), 137 | 2 * s * (q[2] * q[3] - q[1] * q[0])], 138 | [2 * s * (q[1] * q[3] - q[2] * q[0]), 2 * s * (q[2] * q[3] + q[1] * q[0]), 139 | 1 - 2 * s * (q[1] ** 2 + q[2] ** 2)]]) 140 | elif q.ndim == 2: 141 | s = norm(q, axis=1) 142 | R = array([[1 - 2 * s * (q[:, 2]**2 + q[:, 3]**2), 2 * s * (q[:, 1] * q[:, 2] - q[:, 3] * q[:, 0]), 143 | 2 * s * (q[:, 1] * q[:, 3] + q[:, 2] * q[:, 0])], 144 | [2 * s * (q[:, 1] * q[:, 2] + q[:, 3] * q[:, 0]), 1 - 2 * s * (q[:, 1]**2 + q[:, 3]**2), 145 | 2 * s * (q[:, 2] * q[:, 3] - q[:, 1] * q[:, 0])], 146 | [2 * s * (q[:, 1] * q[:, 3] - q[:, 2] * q[:, 0]), 2 * s * (q[:, 2] * q[:, 3] + q[:, 1] * q[:, 0]), 147 | 1 - 2 * s * (q[:, 1]**2 + q[:, 2]**2)]]) 148 | R = R.transpose([2, 0, 1]) 149 | return R 150 | 151 | 152 | def quat_mean(q): 153 | """ 154 | Calculate the mean of an array of quaternions 155 | 156 | Parameters 157 | ---------- 158 | q : numpy.ndarray 159 | Nx4 array of quaternions 160 | 161 | Returns 162 | ------- 163 | q_mean : numpy.array 164 | Mean quaternion 165 | """ 166 | M = q.T @ q 167 | 168 | vals, vecs = eig(M) # Eigenvalues and vectors of M 169 | sort_ind = argsort(vals) # get the indices of the eigenvalues sorted 170 | 171 | q_mean = real(vecs[:, sort_ind[-1]]) 172 | 173 | # ensure no discontinuities 174 | if q_mean[0] < 0: 175 | q_mean *= -1 176 | 177 | return q_mean 178 | 179 | 180 | def vec2quat(v1, v2): 181 | """ 182 | Find the rotation quaternion between two vectors. Rotate v1 onto v2 183 | 184 | Parameters 185 | ---------- 186 | v1 : numpy.ndarray 187 | Vector 1 188 | v2 : numpy.ndarray 189 | Vector 2 190 | 191 | Returns 192 | ------- 193 | q : numpy.ndarray 194 | Quaternion representing the rotation from v1 to v2 195 | """ 196 | if allclose(v1, v2): 197 | return array([1, 0, 0, 0]) 198 | else: 199 | angle = arccos(dot(v1.flatten(), v2.flatten()) / (norm(v1) * norm(v2))) 200 | 201 | # Rotation axis is always normal to two vectors 202 | axis = cross(v1.flatten(), v2.flatten()) 203 | axis = axis / norm(axis) # normalize 204 | 205 | q = zeros(4) 206 | q[0] = cos(angle / 2) 207 | q[1:] = axis * sin(angle / 2) 208 | q /= norm(q) 209 | 210 | return q 211 | -------------------------------------------------------------------------------- /pykinematics/omc/__init__.py: -------------------------------------------------------------------------------- 1 | from pykinematics.omc import angles 2 | from pykinematics.omc import calibration 3 | from pykinematics.omc import segmentFrames 4 | from pykinematics.omc.utility import MarkerNames, default_marker_names 5 | -------------------------------------------------------------------------------- /pykinematics/omc/angles.py: -------------------------------------------------------------------------------- 1 | """ 2 | Methods for calculating OMC joint angles 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | V0.1 - April 10, 2019 7 | """ 8 | from numpy import cross, sum as nsum, arctan2 as atan2, arccos, abs as nabs, pi, stack 9 | from numpy.linalg import norm 10 | 11 | 12 | def hip(pelvis_frames, thigh_frames, side): 13 | """ 14 | Compute hip angles. 15 | 16 | Parameters 17 | ---------- 18 | pelvis_frames : numpy.ndarray 19 | Nx3x3 array of anatomical frames of the pelvis in a world frame. N time samples of 3x3 matrices, of which the 20 | first column is the pelvis X-axis, second column is the pelvis Y-axis, and third column is the pelvis Z-axis 21 | thigh_frames: numpy.ndarray 22 | Nx3x3 array of anatomical frames of the thigh for N time points in the world frame. Each 3x3 matrix is comprised 23 | of columns of thigh x-axis, y-axis, and z-axis, in that order. 24 | side : {'left', 'right'} 25 | Side the angles are being computed for. Used for sign determination of angles. 26 | 27 | Returns 28 | ------- 29 | hip_angles : numpy.ndarray 30 | Nx3 array of hip angles, with the first column being flexion-extension, second column being ad/abduction, 31 | and third column being internal-external rotation. 32 | 33 | References 34 | ---------- 35 | Wu et al. "ISB recommendations on definitions of joint coordinate systems of various joints for the reporting of 36 | human joint motion - part I: ankle, hip, and spine." J. of Biomech. Vol. 35. 2002. 37 | Dabirrahmani et al. "Modification of the Grood and Suntay Joint Coordinate System equations for knee joint flexion." 38 | Med. Eng. and Phys. Vol. 39. 2017. 39 | Grood et al. "A joint coordinate system for the clinical description of three-dimensional motions: application to 40 | the knee." J. of Biomech. Engr. Vol. 105. 1983. 41 | """ 42 | # extract the proximal (pelvis) segment axes 43 | X = pelvis_frames[:, :, 0] 44 | Z = pelvis_frames[:, :, 2] 45 | 46 | # extract the distal (thigh) segment axes 47 | x = thigh_frames[:, :, 0] 48 | y = thigh_frames[:, :, 1] 49 | 50 | # create the hip joint axes 51 | e1 = Z.copy() 52 | e3 = y.copy() 53 | 54 | e2 = cross(e3, e1) 55 | e2 /= norm(e2, axis=1, keepdims=True) 56 | 57 | # compute the angles by finding the angle between specific axes 58 | sgn = nsum(cross(X, e2) * Z, axis=1) 59 | sgn /= nabs(sgn) 60 | fe = atan2(sgn * norm(cross(X, e2), axis=1), nsum(X * e2, axis=1)) 61 | 62 | if side.lower() == 'right': 63 | sgn = nsum(cross(x, e2) * -y, axis=1) 64 | elif side.lower() == 'left': 65 | sgn = nsum(cross(x, e2) * y, axis=1) 66 | else: 67 | raise ValueError('Side must be "left" or "right".') 68 | 69 | sgn /= nabs(sgn) 70 | ier = atan2(sgn * norm(cross(x, e2), axis=1), nsum(x * e2, axis=1)) 71 | 72 | if side.lower() == 'right': 73 | aa = -pi / 2 + arccos(nsum(e1 * e3, axis=1)) 74 | elif side.lower() == 'left': 75 | aa = pi / 2 - arccos(nsum(e1 * e3, axis=1)) 76 | 77 | return stack((fe, aa, ier), axis=1) * 180 / pi -------------------------------------------------------------------------------- /pykinematics/omc/calibration.py: -------------------------------------------------------------------------------- 1 | """ 2 | Methods for calibration of OMC data. 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | V0.1 - April 10, 2019 7 | """ 8 | from numpy import cross, stack, zeros, isnan, sum as nsum, array, mean 9 | from numpy.linalg import norm, solve 10 | 11 | from pykinematics.omc import utility 12 | from pykinematics.omc import segmentFrames 13 | from pykinematics import common 14 | 15 | 16 | def compute_hip_center(pelvis_data, thigh_data, R, origin, marker_names='default', tol=1e-4): 17 | """ 18 | Compute the hip joint center location using a bias compensated least squares estimation. 19 | 20 | Parameters 21 | ---------- 22 | pelvis_data : dict 23 | Dictionary of all the marker data for the pelvis. Each key is a marker name. 24 | thigh_data : dict 25 | Dictionary of all the marker data for the thigh. Each key is a marker name 26 | R : numpy.ndarray 27 | Rotation matrix or array of rotation matrices representing the rotation from world to local reference frame. 28 | origin : numpy.ndarray 29 | Vector of the origin position to use when rotating and transforming the marker positions into a local 30 | reference frame. 31 | marker_names : {'default', MarkerNames}, optional 32 | Either 'default' to use the default marker names, or a MarkerNames object with the names used in pelvis_data 33 | and thigh_data specified. 34 | tol : float, optional 35 | Tolerance for the bias compensation of the joint center. Default is 1e-4. 36 | 37 | Returns 38 | ------- 39 | c : numpy.ndarray 40 | Vector from the provided origin to the joint center, expressed in the local reference frame. 41 | 42 | References 43 | ---------- 44 | Halvorsen, Kjartan. "Bias compensated least squares estimate of the center of rotation." *J. of Biomech*. 45 | Vol. 36. 2003. 46 | Gamage et al. "New least squares solution for estimating the average center of rotation and the axis of rotation." 47 | *J. of Biomech*. Vol. 35. 2002 48 | """ 49 | if marker_names == 'default': 50 | names = utility.MarkerNames() 51 | else: 52 | names = marker_names 53 | 54 | # create a mask for NaN values 55 | mask = ~isnan(R).any(axis=2).any(axis=1) # check the world to local frame rotation 56 | mask &= ~isnan(pelvis_data[names.pelvis_c2]).any(axis=1) # check the thigh cluster origin 57 | 58 | # check all the marker data for nan as all the data has to be the same length 59 | for mkr in thigh_data.keys(): 60 | mask &= ~isnan(thigh_data[mkr]).any(axis=1) 61 | 62 | # translate and rotate all the thigh marker coordinates into the cluster reference frame 63 | thigh_rot = dict() 64 | for mkr in thigh_data.keys(): 65 | thigh_rot[mkr] = (R[mask] @ (thigh_data[mkr][mask] 66 | - origin[mask]).reshape((-1, 3, 1))).reshape((-1, 3)) 67 | 68 | n = mask.sum() # number of samples 69 | m = len(thigh_rot.keys()) # number of markers 70 | 71 | # create the vectors to store the norms 72 | norm1 = zeros((m, 3, 1)) 73 | norm2 = zeros((m, 1, 1)) 74 | norm3 = zeros((m, 3, 1)) 75 | 76 | for i, mk in enumerate(thigh_rot.keys()): 77 | norm1[i] = thigh_rot[mk].sum(axis=0).reshape((-1, 3, 1)) 78 | norm2[i] = nsum(thigh_rot[mk].reshape((-1, 1, 3)) @ thigh_rot[mk].reshape((-1, 3, 1)), axis=0) 79 | norm3[i] = nsum(thigh_rot[mk].reshape((-1, 3, 1)) @ thigh_rot[mk].reshape((-1, 1, 3)) 80 | @ thigh_rot[mk].reshape((-1, 3, 1)), axis=0) 81 | norm1 /= n 82 | norm2 /= n 83 | norm3 /= n 84 | 85 | A = zeros((3, 3)) 86 | b = zeros((3, 1)) 87 | 88 | for i, mk in enumerate(thigh_rot.keys()): 89 | A += nsum(thigh_rot[mk].reshape((-1, 3, 1)) @ thigh_rot[mk].reshape((-1, 1, 3)), axis=0) / n 90 | A -= norm1[i] @ norm1[i].T 91 | 92 | b += norm3[i] - norm1[i] * norm2[i] 93 | 94 | A *= 2 95 | 96 | # solve for the initial guess of the joint center location in the pelvis cluster reference frame 97 | c = solve(A, b) 98 | 99 | # BIAS COMPENSATION 100 | delta_c = array([1, 1, 1]) # change in joint center coordinates 101 | while (delta_c > tol).any(): 102 | sigma2 = zeros(len(thigh_rot.keys())) 103 | for i, mk in enumerate(thigh_rot.keys()): 104 | u = thigh_rot[mk].reshape((-1, 3, 1)) - c 105 | u2bar = 1 / u.shape[0] * nsum(u.transpose([0, 2, 1]) @ u, axis=0) 106 | sigma2[i] = 1 / (4 * u2bar * u.shape[0]) * nsum((nsum(u * u, axis=1) - u2bar) ** 2) 107 | 108 | sigma2avg = mean(sigma2) 109 | delta_b = zeros((3, 1)) 110 | for mk in thigh_rot.keys(): 111 | delta_b += 1 / thigh_rot[mk].shape[0] * nsum(thigh_rot[mk].reshape((-1, 3, 1)) - c, axis=0) 112 | 113 | delta_b *= 2 * sigma2avg 114 | 115 | cnew = solve(A, b - delta_b) 116 | 117 | delta_c = (cnew - c) / c 118 | c = cnew.copy() 119 | 120 | return c 121 | 122 | 123 | def static(static_data, hip_center_data, window, marker_names='default'): 124 | """ 125 | Process data from a static calibration trial to create the anatomical frames and create constant cluster to 126 | anatomical frame rotations. 127 | 128 | Parameters 129 | ---------- 130 | static_data : tuple 131 | 3-tuple of dictionaries. The first is the dictionary of pelvis data, the second is the dictionary of 132 | left-thigh data, and the third is the dictionary of right-thigh data during a static standing trial. 133 | hip_center_data : tuple 134 | 3-tuple of dictionaries. The first is the dictionary of pelvis data, the second is the dictionary of 135 | left-thigh data, and the third is the dictionary of right-thigh data during a trial where the hip 136 | joint center can be calculated (enough rotation in axes is present to compute the joint center). 137 | window : int 138 | Number of samples to window over for finding the most still section of data. Suggested is 1 second worth of 139 | samples. 140 | marker_names : {'default', pymotion.omc.utility.MarkerNames}, optional 141 | Either 'default', which will use the default marker names, or a modified MarkerNames object, with marker names 142 | as used in the keys of pelvis_data, left_thigh_data, and right_thigh_data. 143 | 144 | Returns 145 | ------- 146 | pelvis : tuple 147 | 3-tuple of pelvis anatomical frame, rotation from world to pelvis cluster frame, and constant rotation from 148 | segment frame to cluster frame. 149 | left_thigh : tuple 150 | 3-tuple of left thigh anatomical frame, rotation from world to the left thigh cluster frame, and constant 151 | rotation from segment frame to cluster frame. 152 | right_thigh : tuple 153 | 3-tuple of right thigh anatomical frame, rotation from world to the right thigh cluster frame, and constant 154 | rotation from segment frame to cluster frame. 155 | """ 156 | # extract the data from the tuples 157 | pelvis_data, left_thigh_data, right_thigh_data = static_data 158 | pelvis_jc_data, left_thigh_jc_data, right_thigh_jc_data = hip_center_data 159 | 160 | # separate the data from the names 161 | raw_data = tuple(pelvis_data[name] for name in pelvis_data.keys()) \ 162 | + tuple(left_thigh_data[name] for name in left_thigh_data.keys()) \ 163 | + tuple(right_thigh_data[name] for name in right_thigh_data.keys()) 164 | raw_names = list(pelvis_data.keys()) + list(left_thigh_data.keys()) + list(right_thigh_data.keys()) 165 | 166 | # find the most still period in the data, and the index of that window 167 | still_data, still_ind = common.find_most_still(raw_data, window, return_index=True) 168 | 169 | # associate still data with the names 170 | markers = dict() 171 | for data, name in zip(still_data, raw_names): 172 | markers[name] = data 173 | 174 | # set the marker names 175 | if marker_names == 'default': 176 | names = utility.MarkerNames() 177 | else: 178 | names = marker_names 179 | 180 | # ----------------------------------------------- 181 | # PELVIS 182 | # ----------------------------------------------- 183 | # compute the pelvis origin, will be needed later 184 | pelvis_o = utility.compute_pelvis_origin(markers[names.left_asis], markers[names.right_asis]) 185 | 186 | # create a matrix with columns as the x, y, z, axes of the anatomical frame 187 | # this is also the rotation matrix from pelvis frame to world frame 188 | pelvis_af = segmentFrames.pelvis(markers, use_cluster=False, marker_names=names) 189 | 190 | # create the cluster frame 191 | pelvis_R_c_w = utility.create_cluster_frame(markers, 'pelvis', marker_names=names) 192 | 193 | # create the constant segment to cluster rotation matrix 194 | pelvis_R_s_c = pelvis_R_c_w.T @ pelvis_af 195 | 196 | # ----------------------------------------------- 197 | # JOINT CENTERS 198 | # ----------------------------------------------- 199 | # first compute the anatomical frame using the data provided for joint center computation 200 | pelvis_jc_af = segmentFrames.pelvis(pelvis_jc_data, use_cluster=False, marker_names=names) 201 | 202 | # compute the origin using the data provided for joint center computation 203 | pelvis_jc_o = utility.compute_pelvis_origin(pelvis_jc_data[names.left_asis], pelvis_jc_data[names.right_asis]) 204 | 205 | # compute the joint center locations in the pelvis frame 206 | right_jc_p = compute_hip_center(pelvis_jc_data, right_thigh_jc_data, pelvis_jc_af.transpose([0, 2, 1]), pelvis_jc_o, 207 | marker_names=names) 208 | left_jc_p = compute_hip_center(pelvis_jc_data, left_thigh_jc_data, pelvis_jc_af.transpose([0, 2, 1]), pelvis_jc_o, 209 | marker_names=names) 210 | 211 | # ----------------------------------------------- 212 | # Right THIGH 213 | # ----------------------------------------------- 214 | # first transform the left hip joint center coordinates back into the world frame 215 | right_hjc = pelvis_af @ right_jc_p.flatten() + pelvis_o 216 | 217 | # create the anatomical frame matrix, also the left thigh to world rotation matrix 218 | rthigh_af = segmentFrames.thigh(markers, 'right', use_cluster=False, hip_joint_center=right_hjc, marker_names=names) 219 | 220 | # compute the cluster orientation, also cluster to world 221 | rthigh_R_c_w = utility.create_cluster_frame(markers, 'right_thigh', names) 222 | 223 | # compute the constant segment to cluster rotation matrix 224 | rthigh_R_s_c = rthigh_R_c_w.T @ rthigh_af 225 | 226 | # ----------------------------------------------- 227 | # LEFT THIGH 228 | # ----------------------------------------------- 229 | # first transform the left hip joint center coordinates back into the world frame 230 | left_hjc = pelvis_af @ left_jc_p.flatten() + pelvis_o 231 | 232 | # create the anatomical frame matrix, also the left thigh to world rotation matrix 233 | lthigh_af = segmentFrames.thigh(markers, 'left', use_cluster=False, hip_joint_center=left_hjc, marker_names=names) 234 | 235 | # compute the cluster orientation, also cluster to world rotation 236 | lthigh_R_c_w = utility.create_cluster_frame(markers, 'left_thigh', names) 237 | 238 | # compute the constant segment to cluster rotation matrix 239 | lthigh_R_s_c = lthigh_R_c_w.T @ lthigh_af 240 | 241 | return (pelvis_af, pelvis_R_c_w, pelvis_R_s_c), (lthigh_af, lthigh_R_c_w, lthigh_R_s_c), (rthigh_af, rthigh_R_c_w, 242 | rthigh_R_s_c) 243 | 244 | 245 | -------------------------------------------------------------------------------- /pykinematics/omc/segmentFrames.py: -------------------------------------------------------------------------------- 1 | """ 2 | Creation of anatomical segment frames from optical motion capture data 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | V0.1 - April 10, 2019 7 | """ 8 | from numpy import cross, stack 9 | from numpy.linalg import norm 10 | 11 | from pykinematics.omc import utility 12 | 13 | 14 | def pelvis(marker_data, use_cluster=False, R_s_c=None, marker_names='default'): 15 | """ 16 | Create the pelvis anatomical frame. 17 | 18 | Parameters 19 | ---------- 20 | marker_data : dictionary 21 | Dictionary of marker position data. Keys correspond to marker names 22 | use_cluster : bool, optional 23 | Use the cluster to segment rotation to compute the anatomical frame. Default is False. 24 | R_s_c : {None, numpy.ndarray}, optional 25 | If use_cluster is False, R_s_c is ignored. If use_cluster is True, then a 3x3 rotation matrix must be provided 26 | that is the rotation from the segment to cluster frame for the pelvis. 27 | marker_names : {'default', pymotion.omc.utility.MarkerNames}, optional 28 | Either 'default', which will use the default marker names, or a modified MarkerNames object, with marker names 29 | as used in the keys of pelvis_data, left_thigh_data, and right_thigh_data. 30 | 31 | Returns 32 | ------- 33 | pelvis_af : numpy.ndarray 34 | 3x3 matrix representing the pelvis anatomical frame. Also is the rotation from world frame into the pelvis 35 | anatomical frame. Columns are the x, y, z axes of the anatomical frame. 36 | """ 37 | # get marker names 38 | if marker_names == 'default': 39 | names = utility.MarkerNames 40 | else: 41 | names = marker_names 42 | 43 | if use_cluster: 44 | # first compute the cluster orientation 45 | R_c_w = utility.create_cluster_frame(marker_data, 'pelvis', names) 46 | 47 | # compute the anatomical frame matrix, which is the world to segment rotation matrix 48 | pelvis_af = R_c_w @ R_s_c 49 | 50 | else: 51 | # compute the pelvis origin 52 | origin = utility.compute_pelvis_origin(marker_data[names.left_asis], marker_data[names.right_asis]) 53 | 54 | z = marker_data[names.right_asis] - marker_data[names.left_asis] 55 | z /= norm(z) if z.ndim == 1 else norm(z, axis=1, keepdims=True) 56 | 57 | mid_psis = (marker_data[names.right_psis] + marker_data[names.left_psis]) / 2 58 | 59 | x_tmp = origin - mid_psis 60 | 61 | y = cross(z, x_tmp) 62 | y /= norm(y) if y.ndim == 1 else norm(y, axis=1, keepdims=True) 63 | 64 | x = cross(y, z) 65 | x /= norm(x) if x.ndim == 1 else norm(x, axis=1, keepdims=True) 66 | 67 | # create a matrix with columns as the x, y, z, axes of the anatomical frame 68 | pelvis_af = stack((x, y, z), axis=1) if x.ndim == 1 else stack((x, y, z), axis=2) 69 | 70 | return pelvis_af 71 | 72 | 73 | def thigh(marker_data, side, use_cluster=True, R_s_c=None, hip_joint_center=None, marker_names='default'): 74 | """ 75 | Create the pelvis anatomical frame. 76 | 77 | Parameters 78 | ---------- 79 | marker_data : dictionary 80 | Dictionary of thigh marker position data. Keys correspond to marker names 81 | side : {'left', 'right'} 82 | Thigh side. 83 | use_cluster : bool, optional 84 | Use the cluster to segment rotation to compute the anatomical frame. Default is True. 85 | R_s_c : {None, numpy.ndarray}, optional 86 | If use_cluster is False, R_s_c is ignored. If use_cluster is True, then a 3x3 rotation matrix must be provided 87 | that is the rotation from the segment to cluster frame for the pelvis. 88 | hip_joint_center : {None, numpy.ndarray}, optional 89 | If use_cluster is False, hip_joint_center must be the location of the hip joint center in the world frame. If 90 | use_cluster is True, then it is ignored. 91 | marker_names : {'default', pymotion.omc.utility.MarkerNames}, optional 92 | Either 'default', which will use the default marker names, or a modified MarkerNames object, with marker names 93 | as used in the keys of marker_data 94 | 95 | Returns 96 | ------- 97 | thigh_af : numpy.ndarray 98 | 3x3 matrix representing the thigh anatomical frame. Also is the rotation from world frame into the thigh 99 | anatomical frame. Columns are the x, y, z axes of the anatomical frame. 100 | """ 101 | # get marker names 102 | if marker_names == 'default': 103 | names = utility.MarkerNames 104 | else: 105 | names = marker_names 106 | 107 | if use_cluster: 108 | # first compute the cluster orientation 109 | if side == 'right': 110 | segment_name = 'right_thigh' 111 | elif side == 'left': 112 | segment_name = 'left_thigh' 113 | else: 114 | raise ValueError('side must be either "left" or "right".') 115 | R_c_w = utility.create_cluster_frame(marker_data, segment_name, names) 116 | 117 | # compute the anatomical frame matrix, which is the segment to world rotation matrix 118 | thigh_af = R_c_w @ R_s_c 119 | 120 | else: 121 | # compute the midpoint of the epicondyles 122 | if side =='right': 123 | mid_ep = (marker_data[names.right_lep] + marker_data[names.right_mep]) / 2 124 | elif side == 'left': 125 | mid_ep = (marker_data[names.left_lep] + marker_data[names.left_mep]) / 2 126 | else: 127 | raise ValueError('side must be either "left" or "right".') 128 | 129 | # create the axes 130 | y = hip_joint_center - mid_ep 131 | y /= norm(y) 132 | 133 | if side == 'right': 134 | z_tmp = marker_data[names.right_lep] - marker_data[names.right_mep] 135 | elif side == 'left': 136 | z_tmp = marker_data[names.left_mep] - marker_data[names.left_lep] 137 | 138 | 139 | x = cross(y, z_tmp) 140 | x /= norm(x) 141 | 142 | z = cross(x, y) 143 | z /= norm(z) 144 | 145 | # create the anatomical frame matrix, also the world to left thigh rotation matrix 146 | thigh_af = stack((x, y, z), axis=1) 147 | 148 | return thigh_af 149 | 150 | -------------------------------------------------------------------------------- /pykinematics/omc/utility.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utility methods for OMC joint angle estimation 3 | 4 | GNU GPL v3.0 5 | Lukas Adamowicz 6 | V0.1 - April 10, 2019 7 | """ 8 | from numpy import cross, stack 9 | from numpy.linalg import norm 10 | from dataclasses import dataclass 11 | 12 | 13 | @dataclass 14 | class MarkerNames: 15 | """ 16 | Data class for storing the labels of specific markers needed to calculate hip joint angles. Each class attribute 17 | is the name of a specific marker location, and its value is the name of that marker in the optical motion capture 18 | data used to estimate hip joint angles. 19 | 20 | To get a list of the markers, use :meth:`pykinematics.omc.default_marker_names`. 21 | """ 22 | # pelvis markers 23 | left_asis: str = 'left_asis' 24 | right_asis: str = 'right_asis' 25 | left_psis: str = 'left_psis' 26 | right_psis: str = 'right_psis' 27 | 28 | # pelvis cluster markers 29 | pelvis_c1: str = 'sacrum_cluster1' 30 | pelvis_c2: str = 'sacrum_cluster2' 31 | pelvis_c3: str = 'sacrum_cluster3' 32 | 33 | # left thigh markers 34 | left_lep: str = 'left_lat_femoral_epicondyle' 35 | left_mep: str = 'left_med_femoral_epicondyle' 36 | 37 | # left thigh cluster markers 38 | left_thigh_c1: str = 'left_thigh_cluster1' 39 | left_thigh_c2: str = 'left_thigh_cluster2' 40 | left_thigh_c3: str = 'left_thigh_cluster3' 41 | 42 | # right thigh markers 43 | right_lep: str = 'right_lat_femoral_epicondyle' 44 | right_mep: str = 'right_med_femoral_epicondyle' 45 | 46 | # right thigh cluster markers 47 | right_thigh_c1: str = 'right_thigh_cluster1' 48 | right_thigh_c2: str = 'right_thigh_cluster2' 49 | right_thigh_c3: str = 'right_thigh_cluster3' 50 | 51 | 52 | def default_marker_names(): 53 | """ 54 | Print and return the default marker names. 55 | 56 | Returns 57 | ------- 58 | marker_names : pykinematics.omc.MarkerNames 59 | Object for storing the marker names and their associated label in the optical motion capture marker data. 60 | """ 61 | for key in MarkerNames.__dataclass_fields__.keys(): 62 | print(f'{key}: {MarkerNames.__dataclass_fields__[key].name}') 63 | 64 | return MarkerNames 65 | 66 | 67 | def create_cluster_frame(marker_data, segment_name, marker_names='default'): 68 | """ 69 | Create a cluster reference frame for the specified segment. 70 | 71 | Parameters 72 | ---------- 73 | marker_data : dict 74 | Dictionary of marker data, where keys are the marker names. 75 | segment_name : {'pelvis', 'left_thigh', 'right_thigh', 'left_shank', 'right_shank'} 76 | Name of the segment to calculate the cluster for. Must match a prefix of a cluster name in marker_names 77 | marker_names : {'default', MarkerNames}, optional 78 | Either 'default' which will use the default marker names (see MarkerNames class), or a modified MarkerNames 79 | object where the names are changed to match those in the marker_data keys. 80 | 81 | Returns 82 | ------- 83 | R_w_c : numpy.ndarray 84 | Either a 3x3 or Nx3x3 array of rotation matrices of the rotation from the world frame to the cluster frame. 85 | The first, second, and third columns are also the cluster frame x, y, and z axes respectively. 86 | """ 87 | # get the marker names to use 88 | if marker_names == 'default': 89 | names = MarkerNames() 90 | else: 91 | names = marker_names 92 | 93 | # create the cluster names 94 | name_c1 = segment_name + '_c1' 95 | name_c2 = segment_name + '_c2' 96 | name_c3 = segment_name + '_c3' 97 | 98 | # create the cluster frame 99 | x = marker_data[names.__dict__[name_c3]] - marker_data[names.__dict__[name_c2]] 100 | x /= norm(x) if x.ndim == 1 else norm(x, axis=1, keepdims=True) 101 | 102 | y_tmp = marker_data[names.__dict__[name_c1]] - marker_data[names.__dict__[name_c2]] 103 | 104 | z = cross(x, y_tmp) 105 | z /= norm(z) if z.ndim == 1 else norm(z, axis=1, keepdims=True) 106 | 107 | y = cross(z, x) 108 | y /= norm(y) if y.ndim == 1 else norm(y, axis=1, keepdims=True) 109 | 110 | # create and return the matrix of axes 111 | R_w_c = stack((x, y, z), axis=1) if x.ndim == 1 else stack((x, y, z), axis=2) 112 | return R_w_c 113 | 114 | 115 | def compute_pelvis_origin(left_asis, right_asis): 116 | """ 117 | Compute the origin of the pelvis. 118 | 119 | Parameters 120 | ---------- 121 | left_asis : numpy.ndarray 122 | 1 or 2D (Nx3) array of positions of the left asis marker. 123 | right_asis : numpy.ndarray 124 | 1 or 2D (Nx3) array of positions of the right asis marker. 125 | 126 | Returns 127 | ------- 128 | origin : numpy.ndarray 129 | Array of the position of the pelvis origin. 130 | 131 | References 132 | ---------- 133 | Ren et al. "Whole body inverse dynamics over a complete gait cycle based only on measured kinematics." 134 | *J. of Biomech*. 2008 135 | """ 136 | origin = (left_asis + right_asis) / 2 137 | 138 | return origin 139 | 140 | 141 | def compute_thigh_origin(lat_fem_ep, med_fem_ep): 142 | """ 143 | Compute the origin of the thigh. 144 | 145 | Parameters 146 | ---------- 147 | lat_fem_ep : numpy.ndarray 148 | 1 or 2D (Nx3) array of positions of the lateral femoral epicondyle marker. 149 | med_fem_ep : numpy.ndarray 150 | 1 or 2D (Nx3) array of positions of the medial femoral epicondyle marker. 151 | 152 | Returns 153 | ------- 154 | origin : numpy.ndarray 155 | Array of the position of the thigh origin 156 | 157 | References 158 | ---------- 159 | Ren et al. "Whole body inverse dynamics over a complete gait cycle based only on measured kinematics." 160 | *J. of Biomech*. 2008 161 | """ 162 | origin = (lat_fem_ep + med_fem_ep) / 2 163 | 164 | return origin 165 | 166 | 167 | def compute_shank_origin(lat_mall, med_mall): 168 | """ 169 | Compute the origin of the shank. 170 | 171 | Parameters 172 | ---------- 173 | lat_mall : numpy.ndarray 174 | 1 or 2D (Nx3) array of positions of the lateral malleolus marker. 175 | med_mall : numpy.ndarray 176 | 1 or 2D (Nx3) array of positions of the medial malleolus marker. 177 | 178 | Returns 179 | ------- 180 | origin : numpy.ndarray 181 | Array of the position of the pelvis origin. 182 | 183 | References 184 | ---------- 185 | Ren et al. "Whole body inverse dynamics over a complete gait cycle based only on measured kinematics." 186 | *J. of Biomech*. 2008 187 | """ 188 | origin = (lat_mall + med_mall) / 2 189 | 190 | return origin 191 | -------------------------------------------------------------------------------- /pykinematics/tests/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/M-SenseResearchGroup/pykinematics/02d61da3cfc1e5562437523e35f4ff996f9a5bdd/pykinematics/tests/__init__.py -------------------------------------------------------------------------------- /pykinematics/tests/__main__.py: -------------------------------------------------------------------------------- 1 | if __name__ == '__main__': 2 | import sys 3 | import os 4 | import pytest 5 | 6 | errcode = pytest.main([os.path.dirname(__file__)] + sys.argv[1:]) 7 | sys.exit(errcode) 8 | -------------------------------------------------------------------------------- /pykinematics/tests/conftest.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | from numpy import array 3 | 4 | 5 | def pytest_addoption(parser): 6 | parser.addoption( 7 | "--no-integration", action="store_true", default=False, help="Don't run integration testing" 8 | ) 9 | 10 | 11 | def pytest_configure(config): 12 | config.addinivalue_line("markers", "integration: mark test as an integration test") 13 | 14 | 15 | def pytest_collection_modifyitems(config, items): 16 | if not config.getoption('--no-integration'): 17 | return # --no-integration wasn't given, run all tests 18 | skip_integration = pytest.mark.skip(reason="need --no-integration option to not run") 19 | for item in items: 20 | if "integration" in item.keywords: 21 | item.add_marker(skip_integration) 22 | 23 | 24 | @pytest.fixture 25 | def simple_x(): 26 | return array([1, 9, 4, 2, 1, 3, 5, 4, 3, 5, 1, 3, 8, 4, 7, 6, 1, 2]) 27 | 28 | 29 | @pytest.fixture 30 | def ma_window(): 31 | return 5 32 | 33 | 34 | @pytest.fixture 35 | def simple_x_mov_stats(): 36 | mn = array([0.0, 0.0, 0.0, 3.4, 3.8, 3.0, 3.0, 3.2, 4.0, 3.6, 3.2, 4.0, 4.2, 4.6, 5.6, 5.2, 0.0, 0.0]) 37 | sd = array([0.0, 0.0, 0.0, 3.36154726, 3.1144823, 1.58113883, 1.58113883, 1.4832397, 1.0, 1.67332005, 1.4832397, 38 | 2.64575131, 2.58843582, 2.88097206, 2.07364414, 2.77488739, 0.0, 0.0]) 39 | return mn, sd 40 | -------------------------------------------------------------------------------- /pykinematics/tests/imu/conftest.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | from numpy import array, identity, cos, sin, pi, concatenate 3 | import requests 4 | from tempfile import TemporaryFile 5 | 6 | 7 | @pytest.fixture(scope='package') 8 | def sample_file(): 9 | tf = TemporaryFile() # temp file to store data 10 | # pull the data from the web 11 | data = requests.get('https://www.uvm.edu/~rsmcginn/download/sample_data.h5') 12 | tf.write(data.content) 13 | data.close() # close off the connection 14 | 15 | yield tf 16 | tf.close() # on teardown close the tempfile 17 | 18 | 19 | @pytest.fixture 20 | def pelvis_af(): 21 | return array([-0.81886302, 0.18082344, 0.54476256]), array([-0.48946473, -0.71571412, -0.49817424]),\ 22 | array([0.29981268, -0.67457852, 0.67457852]) 23 | 24 | 25 | @pytest.fixture 26 | def left_thigh_af(): 27 | return array([0.03280552, 0.51524739, 0.65611038]), array([-0.99875234, 0, 0.04993762]), \ 28 | array([0.0308188, -0.78684867, 0.61637601]) 29 | 30 | 31 | @pytest.fixture 32 | def right_thigh_af(): 33 | return array([-0.22030363, 0.56599008, 0.50842072]), array([-0.96024973, -0.22331389, -0.16748542]),\ 34 | array([0.02366255, -0.66295973, 0.74828102]) 35 | 36 | 37 | @pytest.fixture 38 | def F(): 39 | return array([0.1, 0.2, 0.3, 0.4, 0.4, 0.3, 0.0, -0.5, -0.4, -0.35, -0.45, -0.8]) 40 | 41 | 42 | @pytest.fixture(params=[2, 4]) 43 | def derivative_result(request): 44 | if request.param == 2: 45 | res = array([1.0, 1.0, 1.0, 0.5, -0.5, -2.0, -4.0, -2.0, 0.75, -0.25, -2.25, -4.75]) 46 | elif request.param == 4: 47 | res = array([1.25, 0.41666667, 1.75, 1.08333333, -0.41666667, -2.75, -5.33333333, -2.70833333, 0.625, 48 | -1.41666667, -0.70833333, -5.08333333]) 49 | return request.param, res 50 | 51 | 52 | @pytest.fixture(scope='module') 53 | def q1n(): 54 | return array([0.5, 0.5, 0.5, 0.5]) 55 | 56 | 57 | @pytest.fixture(scope='module') 58 | def q2n(): 59 | return array([0.26, 0.13, 0.64, -0.71]) 60 | 61 | 62 | @pytest.fixture(scope='module') 63 | def q3n(): 64 | return array([0.77, 0.46, 0.10, 0.43]) 65 | 66 | 67 | @pytest.fixture(scope='module') 68 | def q4_2d(): 69 | return array([[1, 0, 0, 0], [0.26, 0.13, 0.64, -0.71]]) 70 | 71 | 72 | @pytest.fixture(params=['12', '13', '23']) 73 | def qmult(request, q1n, q2n, q3n): 74 | if request.param == '12': 75 | res = array([0.1, -0.48, 0.87, 0.03]) 76 | return q1n, q2n, res 77 | elif request.param == '13': 78 | res = array([-0.11, 0.78, 0.45, 0.42]) 79 | return q1n, q3n, res 80 | elif request.param == '23': 81 | res = array([0.3817, 0.5659, 0.1363, -0.7163]) 82 | return q2n, q3n, res 83 | 84 | 85 | @pytest.fixture(params=['1', '2', '3', '4']) 86 | def qconj(request, q1n, q2n, q3n, q4_2d): 87 | if request.param == '1': 88 | return q1n, array([0.5, -0.5, -0.5, -0.5]) 89 | elif request.param == '2': 90 | return q2n, array([0.26, -0.13, -0.64, 0.71]) 91 | elif request.param == '3': 92 | return q3n, array([0.77, -0.46, -0.10, -0.43]) 93 | elif request.param == '4': 94 | return q4_2d, array([[1, 0, 0, 0], [0.26, -0.13, -0.64, 0.71]]) 95 | 96 | 97 | @pytest.fixture(params=['1', '2', '3']) 98 | def qinv(request, q1n, q2n, q3n): 99 | if request.param == '1': 100 | return q1n 101 | elif request.param == '2': 102 | return q2n 103 | elif request.param == '3': 104 | return q3n 105 | 106 | 107 | @pytest.fixture(params=['1', '2', '3', '4']) 108 | def q2mat(request, q1n, q2n, q3n, q4_2d): 109 | if request.param == '1': 110 | return q1n, array([[0.0, 0.0, 1.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) 111 | elif request.param == '2': 112 | return q2n, array([[-0.8257546, 0.53511774, 0.14806656], [-0.2026174, -0.04106178, -0.97552084], 113 | [-0.51693413, -0.84044258, 0.14776805]]) 114 | elif request.param == '3': 115 | return q3n, array([[0.61031696, -0.57002891, 0.5494351], [0.75397371, 0.20723794, -0.62221325], 116 | [0.24152751, 0.79416164, 0.55693298]]) 117 | elif request.param == '4': 118 | return q4_2d, array([identity(3), [[-0.8257546, 0.53511774, 0.14806656], 119 | [-0.2026174, -0.04106178, -0.97552084], 120 | [-0.51693413, -0.84044258, 0.14776805]]]) 121 | 122 | 123 | @pytest.fixture 124 | def R(): 125 | r1 = array([[1, 0, 0], [0, 0, 1], [0, 1, 0]]).reshape((-1, 3, 3)) 126 | 127 | t2 = 5 * pi / 180 128 | r2 = r1 @ array([[1, 0, 0], [0, cos(t2), -sin(t2)], [0, sin(t2), cos(t2)]]).reshape((-1, 3, 3)) 129 | 130 | t3 = 7.5 * pi / 180 131 | r3 = r1 @ array([[1, 0, 0], [0, cos(t3), -sin(t3)], [0, sin(t2), cos(t3)]]).reshape((-1, 3, 3)) 132 | 133 | return concatenate((r1, r2, r3), axis=0) 134 | -------------------------------------------------------------------------------- /pykinematics/tests/imu/test_imu.py: -------------------------------------------------------------------------------- 1 | """ 2 | Testing of functions and classes for IMU based estimation of joint kinematics 3 | """ 4 | import pytest 5 | from numpy import isclose, random, insert, array, zeros, gradient, random 6 | import h5py 7 | 8 | from pykinematics.imu.utility import * 9 | from pykinematics.imu.orientation import * 10 | from pykinematics.imu.lib.joints import * 11 | from pykinematics.imu.lib.calibration import * 12 | from pykinematics.imu.lib.angles import * 13 | 14 | 15 | class TestImuUtility: 16 | def test_calc_derivative(self, F, derivative_result): 17 | dt = 0.1 18 | f = calc_derivative(F, dt, derivative_result[0]) 19 | 20 | assert allclose(f, derivative_result[1]) 21 | 22 | def test_quat_mult(self, qmult): 23 | assert allclose(quat_mult(qmult[0], qmult[1]), qmult[2]) 24 | 25 | @pytest.mark.parametrize(('q1', 'q2'), ( 26 | (random.rand(4), random.rand(3)), 27 | (random.rand(3), random.rand(3)), 28 | (random.rand(4, 2), random.rand(4, 2)), 29 | (random.rand(8), random.rand(4)))) 30 | def test_quat_mult_error(self, q1, q2): 31 | with pytest.raises(ValueError) as e_info: 32 | quat_mult(q1, q2) 33 | 34 | def test_quat_conj(self, qconj): 35 | assert allclose(quat_conj(qconj[0]), qconj[1]) 36 | 37 | def test_quat_inv(self, qinv): 38 | qn_ = qinv 39 | q_ = qn_ * 3 40 | 41 | assert allclose(quat_mult(qn_, quat_inv(qn_)), array([1, 0, 0, 0])) 42 | assert isclose(norm(qn_) * norm(quat_inv(qn_)), 1.0) 43 | assert allclose(quat_mult(q_, quat_inv(q_)), array([1, 0, 0, 0])) 44 | assert isclose(norm(q_) * norm(quat_inv(q_)), 1.0) 45 | 46 | def test_quat_inv_error(self): 47 | q = random.rand(2, 4) 48 | with pytest.raises(Exception): 49 | quat_inv(q) 50 | 51 | def test_quat2matrix(self, q2mat): 52 | assert allclose(quat2matrix(q2mat[0]), q2mat[1]) 53 | 54 | @pytest.mark.parametrize(('q', 'result'), ( 55 | (array([[1, 0, 0, 0], [0.26, 0.13, 0.64, -0.71]]), array([0.79481443, 0.08177961, 0.40260729, -0.44664246])), 56 | )) 57 | def test_quat_mean(self, q, result): 58 | assert allclose(quat_mean(q), result) 59 | 60 | @pytest.mark.parametrize(('v1', 'v2'), ((random.rand(3), random.rand(3)), 61 | (array([-0.8, .044, 1.34]), array([-0.8, .044, 1.34])))) 62 | def test_vec2quat(self, v1, v2): 63 | v1 /= norm(v1) 64 | v2 /= norm(v2) 65 | 66 | q = vec2quat(v1, v2) 67 | v2_comp = quat_mult(quat_mult(q, insert(v1, 0, 0)), quat_inv(q))[1:] 68 | v1_comp = quat_mult(quat_mult(quat_inv(q), insert(v2, 0, 0)), q)[1:] 69 | 70 | assert allclose(v2, v2_comp) 71 | assert allclose(v1, v1_comp) 72 | 73 | 74 | class TestImuOrientation: 75 | @pytest.mark.integration 76 | def test_madgwick(self, sample_file): 77 | with h5py.File(sample_file, 'r') as f_: 78 | acc = f_['Star Calibration']['Right Thigh']['Accelerometer'][()] 79 | gyr = f_['Star Calibration']['Right Thigh']['Gyroscope'][()] 80 | mag = f_['Star Calibration']['Right Thigh']['Magnetometer'][()] 81 | 82 | # initialize object 83 | ahrs = MadgwickAHRS(sample_period=1/128, q_init=array([1, 0, 0, 0]), beta=0.041) 84 | 85 | for i in range(150): # only run for 150 samples, not need to waste time processing the whole trial 86 | q_mimu = ahrs.update(gyr[i], acc[i], mag[i]) 87 | q_imu = ahrs.updateIMU(gyr[i], acc[i]) 88 | 89 | assert allclose(q_mimu, array([0.99532435, -0.00598765, 0.09583466, 0.01045504])) 90 | assert allclose(q_imu, array([0.99529476, -0.00603338, 0.09613962, 0.0104455])) 91 | 92 | def test_ssro_error(self): 93 | with pytest.raises(ValueError) as e_info: 94 | SSRO(c=1.01) 95 | with pytest.raises(ValueError) as e_info: 96 | SSRO(c=-0.1) 97 | 98 | @pytest.mark.integration 99 | def test_ssro(self, sample_file): 100 | with h5py.File(sample_file, 'r') as f_: 101 | acc_lu = f_['Star Calibration']['Lumbar']['Accelerometer'][()] 102 | gyr_lu = f_['Star Calibration']['Lumbar']['Gyroscope'][()] 103 | mag_lu = f_['Star Calibration']['Lumbar']['Magnetometer'][()] 104 | 105 | acc_rt = f_['Star Calibration']['Right Thigh']['Accelerometer'][()] 106 | gyr_rt = f_['Star Calibration']['Right Thigh']['Gyroscope'][()] 107 | mag_rt = f_['Star Calibration']['Right Thigh']['Magnetometer'][()] 108 | 109 | # initialize object. Set params so if defaults change don't need to re-do 110 | ssro = SSRO(c=0.01, N=64, error_factor=5e-8, sigma_g=1e-3, sigma_a=6e-3, grav=9.81, init_window=8) 111 | 112 | x_ = ssro.run(acc_lu, acc_rt, gyr_lu, gyr_rt, mag_lu, mag_rt, dt=1/128) # run the algorithm 113 | 114 | assert allclose(x_[-1, :3], array([-0.98440996, -0.02239243, 0.17445807])) 115 | assert allclose(x_[-1, 3:6], array([-0.99828522, -0.03859549, -0.04401142])) 116 | assert allclose(x_[-1, 6:], array([0.76327537, -0.6417982, 0.06491315, 0.03594532])) 117 | 118 | 119 | class TestImuJoint: 120 | def test_joint_center_bad_method(self): 121 | with pytest.raises(ValueError) as e_info: 122 | Center(method='not SAC or SSFC') 123 | 124 | def test_joint_center_bad_mask_data(self): 125 | with pytest.raises(ValueError) as e_info: 126 | Center(mask_data='not acc or gyr') 127 | 128 | def test_joint_center_not_enough_samples(self, sample_file): 129 | with h5py.File(sample_file, 'r') as f_: 130 | acc_lu = f_['Star Calibration']['Lumbar']['Accelerometer'][()] 131 | gyr_lu = f_['Star Calibration']['Lumbar']['Gyroscope'][()] 132 | 133 | acc_rt = f_['Star Calibration']['Right Thigh']['Accelerometer'][()] 134 | gyr_rt = f_['Star Calibration']['Right Thigh']['Gyroscope'][()] 135 | 136 | dgyr_lu = gradient(gyr_lu, 1 / 128, axis=0) 137 | dgyr_rt = gradient(gyr_rt, 1 / 128, axis=0) 138 | 139 | jc_comp = Center(g=9.81, method='SAC', mask_input=True, min_samples=5000, mask_data='gyr', opt_kwargs=None) 140 | 141 | with pytest.raises(ValueError) as e_info: 142 | rlu, rrt, res = jc_comp.compute(acc_lu, acc_rt, gyr_lu * 0.001, gyr_rt * 0.001, dgyr_lu, dgyr_rt, None) 143 | 144 | jc_comp.method = 'SSFC' 145 | with pytest.raises(ValueError) as e_info: 146 | rlu, rrt, res = jc_comp.compute(acc_lu, acc_rt, gyr_lu * 0.001, gyr_rt * 0.001, dgyr_lu, dgyr_rt, None) 147 | 148 | @pytest.mark.integration 149 | def test_joint_center_sac(self, sample_file): 150 | with h5py.File(sample_file, 'r') as f_: 151 | acc_lu = f_['Star Calibration']['Lumbar']['Accelerometer'][()] 152 | gyr_lu = f_['Star Calibration']['Lumbar']['Gyroscope'][()] 153 | mag_lu = f_['Star Calibration']['Lumbar']['Magnetometer'][()] 154 | 155 | acc_rt = f_['Star Calibration']['Right Thigh']['Accelerometer'][()] 156 | gyr_rt = f_['Star Calibration']['Right Thigh']['Gyroscope'][()] 157 | mag_rt = f_['Star Calibration']['Right Thigh']['Magnetometer'][()] 158 | 159 | dgyr_lu = gradient(gyr_lu, 1/128, axis=0) 160 | dgyr_rt = gradient(gyr_rt, 1/128, axis=0) 161 | 162 | jc_comp = Center(g=9.81, method='SAC', mask_input=True, min_samples=500, mask_data='gyr', opt_kwargs=None) 163 | 164 | ssro = SSRO(c=0.01, N=64, error_factor=5e-8, sigma_g=1e-3, sigma_a=6e-3, grav=9.81, init_window=8) 165 | 166 | x_ = ssro.run(acc_lu, acc_rt, gyr_lu, gyr_rt, mag_lu, mag_rt, dt=1 / 128) # run the algorithm 167 | R = quat2matrix(x_[:, 6:]) 168 | 169 | # mask gyr data 170 | rlu, rrt, res = jc_comp.compute(acc_lu, acc_rt, gyr_lu, gyr_rt, dgyr_lu, dgyr_rt, R) 171 | 172 | assert allclose(rlu, array([-0.05498745, -0.07630086, 0.02368247])) 173 | assert allclose(rrt, array([0.22075409, 0.02654856, 0.04593395])) 174 | 175 | # mask acc data 176 | jc_comp.mask_data = 'acc' 177 | rlu, rrt, res = jc_comp.compute(acc_lu, acc_rt, gyr_lu, gyr_rt, dgyr_lu, dgyr_rt, R) 178 | 179 | assert allclose(rlu, array([-0.08500494, -0.11536752, 0.06489129])) 180 | assert allclose(rrt, array([0.19339046, 0.02506993, 0.04676906])) 181 | 182 | @pytest.mark.integration 183 | def test_joint_center_ssfc(self, sample_file): 184 | with h5py.File(sample_file, 'r') as f_: 185 | acc_lu = f_['Star Calibration']['Lumbar']['Accelerometer'][()] 186 | gyr_lu = f_['Star Calibration']['Lumbar']['Gyroscope'][()] 187 | 188 | acc_rt = f_['Star Calibration']['Right Thigh']['Accelerometer'][()] 189 | gyr_rt = f_['Star Calibration']['Right Thigh']['Gyroscope'][()] 190 | 191 | dgyr_lu = gradient(gyr_lu, 1/128, axis=0) 192 | dgyr_rt = gradient(gyr_rt, 1/128, axis=0) 193 | 194 | jc_comp = Center(g=9.81, method='SSFC', mask_input=True, min_samples=500, mask_data='gyr', opt_kwargs={}) 195 | 196 | rlu, rrt, res = jc_comp.compute(acc_lu, acc_rt, gyr_lu, gyr_rt, dgyr_lu, dgyr_rt, None) 197 | 198 | assert allclose(rlu, array([-0.11404692, -0.03627268, 0.0426779])) 199 | assert allclose(rrt, array([0.25021739, 0.0281134, 0.06362797])) 200 | 201 | # test with masking acc 202 | jc_comp.mask_data = 'acc' 203 | rlu, rrt, res = jc_comp.compute(acc_lu, acc_rt, gyr_lu, gyr_rt, dgyr_lu, dgyr_rt, None) 204 | 205 | assert allclose(rlu, array([-0.01771183, -0.10908138, 0.02415292])) 206 | assert allclose(rrt, array([0.25077565, 0.02290044, 0.05807483])) 207 | 208 | @pytest.mark.integration 209 | def test_knee_axis(self, sample_file): 210 | with h5py.File(sample_file, 'r') as f_: 211 | gyr_rs = f_['Star Calibration']['Right Shank']['Gyroscope'][()] 212 | 213 | gyr_rt = f_['Star Calibration']['Right Thigh']['Gyroscope'][()] 214 | 215 | ka = KneeAxis(mask_input=True, min_samples=500, opt_kwargs={}) 216 | 217 | jrt, jrs = ka.compute(gyr_rt, gyr_rs) 218 | 219 | assert allclose(jrt, array([0.05436506, 0.13249331, 0.98969185])) 220 | assert allclose(jrs, array([-0.0713455, 0.78421354, 0.61637565])) 221 | 222 | @pytest.mark.parametrize(('c1', 'c2', 'c2s', 'ax'), ( 223 | (array([0.1, -0.05, 0.03]), array([-0.11, -0.3, -0.01]), True, array([-0.01, -0.02, 0.04])), 224 | (array([0.1, -0.05, 0.03]), array([-0.11, -0.3, -0.01]), False, array([-0.21, 0.02, -0.04])) 225 | )) 226 | def test_fixed_axis(self, c1, c2, c2s, ax): 227 | axis = fixed_axis(c1, c2, c2s) 228 | 229 | 230 | class TestImuCalibration: 231 | def test_get_acc_scale(self, sample_file): 232 | with h5py.File(sample_file, 'r') as f_: 233 | acc = f_['Static Calibration']['Lumbar']['Accelerometer'][()] 234 | 235 | scale = get_acc_scale(acc, gravity=9.81) 236 | 237 | assert isclose(scale, 0.9753079830416251) 238 | 239 | @pytest.mark.integration 240 | def test_static_calibration(self, sample_file, pelvis_af, left_thigh_af, right_thigh_af): 241 | with h5py.File(sample_file, 'r') as f_: 242 | stat = {} 243 | star = {} 244 | 245 | for loc in ['Left Thigh', 'Lumbar', 'Right Thigh']: 246 | stat[loc] = {} 247 | star[loc] = {} 248 | for meas in ['Accelerometer', 'Gyroscope', 'Magnetometer']: 249 | stat[loc][meas] = f_['Static Calibration'][loc][meas][()] 250 | star[loc][meas] = f_['Star Calibration'][loc][meas][()] 251 | 252 | ssro = SSRO(c=0.01, N=64, error_factor=5e-8, sigma_g=1e-3, sigma_a=6e-3, grav=9.84, init_window=8) 253 | 254 | lt_l_q = ssro.run(stat['Lumbar']['Accelerometer'], stat['Left Thigh']['Accelerometer'], 255 | stat['Lumbar']['Gyroscope'], stat['Left Thigh']['Gyroscope'], 256 | stat['Lumbar']['Magnetometer'], stat['Left Thigh']['Magnetometer'], dt=1/128) 257 | rt_l_q = ssro.run(stat['Lumbar']['Accelerometer'], stat['Right Thigh']['Accelerometer'], 258 | stat['Lumbar']['Gyroscope'], stat['Right Thigh']['Gyroscope'], 259 | stat['Lumbar']['Magnetometer'], stat['Right Thigh']['Magnetometer'], dt=1 / 128) 260 | 261 | lum_r_r = array([-0.1, -0.05, 0.05]) 262 | lum_r_l = array([0.1, -0.5, 0.5]) 263 | 264 | thi_r_lum_r = array([0.25, 0.2, 0.075]) 265 | thi_r_lum_l = array([0.25, -0.15, 0.04]) 266 | 267 | thi_r_kne_r = array([-0.18, 0.1, 0]) 268 | thi_r_kne_l = array([-0.15, -0.15, 0.06]) 269 | 270 | pelvis_axis = fixed_axis(lum_r_l, lum_r_r, center_to_sensor=True) 271 | l_thigh_axis = fixed_axis(thi_r_kne_l, thi_r_lum_l, center_to_sensor=True) 272 | r_thigh_axis = fixed_axis(thi_r_kne_r, thi_r_lum_r, center_to_sensor=True) 273 | 274 | p_AF, lt_AF, rt_AF = static(lt_l_q, rt_l_q, pelvis_axis, l_thigh_axis, r_thigh_axis, 275 | stat['Lumbar']['Gyroscope'], stat['Left Thigh']['Gyroscope'], 276 | stat['Right Thigh']['Gyroscope'], 128, window=1.0) 277 | 278 | assert all((allclose(i, j) for i, j in zip(p_AF, pelvis_af))) 279 | assert all((allclose(i, j) for i, j in zip(lt_AF, left_thigh_af))) 280 | assert all((allclose(i, j) for i, j in zip(rt_AF, right_thigh_af))) 281 | 282 | 283 | class TestImuJointAngles: 284 | def test_hip_from_frames(self, pelvis_af, left_thigh_af, R): 285 | angles = hip_from_frames(pelvis_af, left_thigh_af, R, side='left', zero_angles=False) 286 | 287 | assert allclose(angles, array([[-61.27792272, -19.45858847, -6.21248057], 288 | [-61.08468402, -19.6292999, -11.27089438], 289 | [-60.98217794, -19.70846086, -13.0045286]])) 290 | -------------------------------------------------------------------------------- /pykinematics/tests/omc/conftest.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | from numpy import array 3 | 4 | 5 | @pytest.fixture 6 | def lumbar_cluster_sample_data(): 7 | cls1 = array([[500, 400, 800], [-50, 300, 400]], dtype=float) 8 | cls3 = array([[500, 300, 1800], [-50, 200, 1400]], dtype=float) 9 | cls2 = array([[0, 300, 800], [-550, 200, 400]], dtype=float) 10 | 11 | return {'sacrum_cluster1': cls1, 'sacrum_cluster2': cls2, 'sacrum_cluster3': cls3} 12 | 13 | 14 | @pytest.fixture 15 | def lumbar_marker_sample(): 16 | data = {'right_asis': array([[600, 500, 1200], [610, 470, 1200]], dtype=float), 17 | 'left_asis': array([[300, 500, 1200], [310, 500, 1180]], dtype=float), 18 | 'right_psis': array([[500, 300, 1200], [520, 310, 1160]], dtype=float), 19 | 'left_psis': array([[400, 300, 1200], [430, 300, 1150]], dtype=float)} 20 | return data 21 | 22 | 23 | @pytest.fixture 24 | def thigh_marker_sample(): 25 | data = {'right_lat_femoral_epicondyle': array([700, 400, 500], dtype=float), 26 | 'right_med_femoral_epicondyle': array([540, 400, 500], dtype=float)} 27 | return data 28 | 29 | 30 | @pytest.fixture 31 | def right_hip_jc(): 32 | return array([620, 400, 1100]) 33 | -------------------------------------------------------------------------------- /pykinematics/tests/omc/test_omc.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | from numpy import allclose, isclose, array, mean, concatenate, argmax 3 | from numpy.linalg import det 4 | 5 | from pykinematics.omc.utility import * 6 | from pykinematics.omc.segmentFrames import * 7 | 8 | 9 | class TestOmcUtility: 10 | def test_create_cluster_frames(self, lumbar_cluster_sample_data): 11 | R = create_cluster_frame(lumbar_cluster_sample_data, segment_name='pelvis', marker_names='default') 12 | 13 | assert allclose(R[0], R[1]) 14 | assert allclose(R[0], array([[0.4472136, 0.87287156, -0.19518001], 15 | [0., 0.21821789, 0.97590007], 16 | [0.89442719, -0.43643578, 0.09759001]])) 17 | 18 | @pytest.mark.parametrize(('left_asis', 'right_asis'), ((array([300, 200, 800]), array([0, 200, 650])), 19 | (array([-150, 325, 920]), array([-430, 25, 330])))) 20 | def test_compute_pelvis_origin(self, left_asis, right_asis): 21 | origin = compute_pelvis_origin(left_asis, right_asis) 22 | 23 | gnd = mean(concatenate((left_asis.reshape((-1, 3)), right_asis.reshape((-1, 3))), axis=0), axis=0) 24 | assert allclose(origin, gnd) 25 | 26 | @pytest.mark.parametrize(('lat_fem_ep', 'med_fem_ep'), ((array([300, 200, 800]), array([0, 200, 650])), 27 | (array([-150, 325, 920]), array([-430, 25, 330])))) 28 | def test_compute_thigh_origin(self, lat_fem_ep, med_fem_ep): 29 | origin = compute_pelvis_origin(lat_fem_ep, med_fem_ep) 30 | 31 | gnd = mean(concatenate((lat_fem_ep.reshape((-1, 3)), med_fem_ep.reshape((-1, 3))), axis=0), axis=0) 32 | assert allclose(origin, gnd) 33 | 34 | @pytest.mark.parametrize(('lat_mall', 'med_mall'), ((array([300, 200, 800]), array([0, 200, 650])), 35 | (array([-150, 325, 920]), array([-430, 25, 330])))) 36 | def test_compute_shank_origin(self, lat_mall, med_mall): 37 | origin = compute_pelvis_origin(lat_mall, med_mall) 38 | 39 | gnd = mean(concatenate((lat_mall.reshape((-1, 3)), med_mall.reshape((-1, 3))), axis=0), axis=0) 40 | assert allclose(origin, gnd) 41 | 42 | 43 | class TestOmcSegmentFrames: 44 | def test_pelvis(self, lumbar_marker_sample): 45 | af = pelvis(lumbar_marker_sample, use_cluster=False, R_s_c=None, marker_names='default') 46 | 47 | assert isclose(det(af[0]), 1.0) 48 | assert isclose(det(af[1]), 1.0) 49 | assert allclose(af[0].astype(int), array([[0, 0, 1], [1, 0, 0], [0, 1, 0]])) 50 | assert all(argmax(af[1], axis=1) == array([2, 0, 1])) 51 | 52 | def test_thigh_side_error(self): 53 | with pytest.raises(ValueError) as e_info: 54 | thigh(None, 'not left/right', use_cluster=False, R_s_c=None, hip_joint_center=None, marker_names='default') 55 | with pytest.raises(ValueError) as e_info: 56 | thigh(None, 'not left/right', use_cluster=True, R_s_c=None, hip_joint_center=None, marker_names='default') 57 | 58 | def test_thigh(self, thigh_marker_sample, right_hip_jc): 59 | af = thigh(thigh_marker_sample, 'right', use_cluster=False, R_s_c=None, hip_joint_center=right_hip_jc, 60 | marker_names='default') 61 | 62 | assert allclose(af, array([[0, 0, 1], [1, 0, 0], [0, 1, 0]])) 63 | -------------------------------------------------------------------------------- /pykinematics/tests/test_common.py: -------------------------------------------------------------------------------- 1 | """ 2 | Testing of functions in pykinematics/common.py 3 | """ 4 | from pykinematics.common import * 5 | 6 | import numpy as np 7 | 8 | 9 | def test_mov_avg_window_1(simple_x): 10 | _, _, pad = mov_avg(simple_x, 1) 11 | 12 | assert pad == 1 13 | 14 | 15 | def test_mov_avg_window_large(simple_x): 16 | mn, sd, pad = mov_avg(simple_x, len(simple_x) * 2 + 4) 17 | 18 | assert pad == 1 19 | assert np.allclose(mn, np.mean(simple_x)) 20 | assert np.allclose(sd, np.std(simple_x, ddof=1)) 21 | 22 | 23 | def test_mov_avg(simple_x, ma_window, simple_x_mov_stats): 24 | x_m, x_sd, pad = mov_avg(simple_x, ma_window) 25 | 26 | assert np.allclose(x_m, simple_x_mov_stats[0]) 27 | assert np.allclose(x_sd, simple_x_mov_stats[1]) 28 | assert np.isclose(pad, 3) 29 | 30 | 31 | def test_find_most_still(): 32 | np.random.seed(5) 33 | x1 = np.random.rand(50, 2) 34 | np.random.seed(34) 35 | x2 = np.random.rand(50, 2) 36 | np.random.seed(13513) 37 | x3 = np.random.rand(50, 2) 38 | 39 | still_data, idx = find_most_still((x1, x2, x3), 10, return_index=True) 40 | result = find_most_still((x1, x2, x3), 10, return_index=False) 41 | 42 | assert np.allclose(np.array(still_data), np.array([[0.49851287, 0.39499911], [0.60036786, 0.45481188], 43 | [0.18751978, 0.54213798]])) 44 | assert np.isclose(idx, 7) 45 | assert len(result) == 3 # should be 3 because thats the length of the input, which is the shape of the return 46 | -------------------------------------------------------------------------------- /pykinematics/tests/test_imports.py: -------------------------------------------------------------------------------- 1 | """ 2 | Testing imports of required libraries 3 | """ 4 | 5 | 6 | def test_numpy(): 7 | import numpy 8 | 9 | return 10 | 11 | 12 | def test_scipy(): 13 | import scipy 14 | 15 | return 16 | 17 | 18 | def test_pykinematics(): 19 | import pykinematics 20 | from pykinematics import imu 21 | from pykinematics import omc 22 | from pykinematics.core import MimuAngles, OmcAngles 23 | 24 | return 25 | 26 | 27 | def test_pykinematics_imu(): 28 | from pykinematics.imu import angles, calibration, joints, optimize, orientation, utility 29 | 30 | return 31 | 32 | 33 | def test_pykinematics_omc(): 34 | from pykinematics.omc import angles, calibration, segmentFrames, MarkerNames, default_marker_names 35 | 36 | return 37 | -------------------------------------------------------------------------------- /pykinematics/version.py: -------------------------------------------------------------------------------- 1 | __version__ = '1.0.0' 2 | -------------------------------------------------------------------------------- /scripts/example_code.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example code for estimating hip joint angles from wearable magnetic and inertial sensors. 3 | 4 | Lukas Adamowicz 5 | 2019 6 | """ 7 | import h5py 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | import pykinematics as pk 12 | 13 | plt.style.use('ggplot') 14 | 15 | 16 | def import_sample_data(path_to_sample_data): 17 | # setup the dictionaries for storing the data 18 | # static calibration: useful to have the shank data to compute a scaling factor to make the gravity vectors similar 19 | stat_data = {'Lumbar': {}, 'Left thigh': {}, 'Right thigh': {}, 'Left shank': {}, 'Right shank': {}} 20 | # star calibration needs the shank data as well, for computation of the joint centers 21 | star_data = {'Lumbar': {}, 'Left thigh': {}, 'Right thigh': {}, 'Left shank': {}, 'Right shank': {}} 22 | walk_data = {'Lumbar': {}, 'Left thigh': {}, 'Right thigh': {}} 23 | 24 | tasks = ['Static Calibration', 'Star Calibration', 'Treadmill Walk Fast'] 25 | signals = [('Acceleration', 'Accelerometer'), ('Angular velocity', 'Gyroscope'), 26 | ('Magnetic field', 'Magnetometer'), ('Time', 'Time')] 27 | with h5py.File(path_to_sample_data, 'r') as file: 28 | for task, dat in zip(tasks, [stat_data, star_data, walk_data]): 29 | for loc in dat.keys(): 30 | for sigs in signals: 31 | dat[loc][sigs[0]] = np.zeros(file[task][loc.title()][sigs[1]].shape) 32 | file[task][loc.title()][sigs[1]].read_direct(dat[loc][sigs[0]]) 33 | for dat in [stat_data, star_data, walk_data]: 34 | for loc in dat.keys(): 35 | dat[loc]['Time'] = dat[loc]['Time'] / 1e6 # convert timestamps to seconds 36 | 37 | return stat_data, star_data, walk_data 38 | 39 | 40 | # import the raw sample data 41 | sample_data_path = 'W:\\Study Data\\Healthy Subjects\\sample_data.h5' 42 | static_calibration_data, star_calibration_data, walk_fast_data = import_sample_data(sample_data_path) 43 | 44 | # define some additional keyword arguments for optimizations and orientation estimation 45 | filt_vals = {'Angular acceleration': (2, 12)} 46 | 47 | ka_kwargs = {'opt_kwargs': {'method': 'trf', 'loss': 'arctan'}} 48 | jc_kwargs = dict(method='SAC', mask_input=True, min_samples=1500, opt_kwargs=dict(loss='arctan'), mask_data='gyr') 49 | orient_kwargs = dict(error_factor=5e-8, c=0.003, N=64, sigma_g=1e-3, sigma_a=6e-3) 50 | 51 | mimu_estimator = pk.ImuAngles(gravity_value=9.8404, filter_values=filt_vals, joint_center_kwargs=jc_kwargs, 52 | orientation_kwargs=orient_kwargs, knee_axis_kwargs=ka_kwargs) 53 | 54 | # calibrate the estimator based on Static and Star Calibration tasks 55 | mimu_estimator.calibrate(static_calibration_data, star_calibration_data) 56 | 57 | # compute the hip joint angles for the Fast Walking on a treadmill 58 | left_hip_angles, right_hip_angles = mimu_estimator.estimate(walk_fast_data, return_orientation=False) 59 | 60 | # PLOTTING 61 | fl, axl = plt.subplots(3, sharex=True) 62 | fr, axr = plt.subplots(3, sharex=True) 63 | label = [r'Flexion/Extension', 'Ad/Abduction', 'Internal/External Rotation'] 64 | for i in range(3): 65 | axl[i].plot(left_hip_angles[:, i]) 66 | axr[i].plot(right_hip_angles[:, i]) 67 | axl[i].set_title(label[i]) 68 | axr[i].set_title(label[i]) 69 | axl[i].set_ylabel('Angle [deg]') 70 | axr[i].set_ylabel('Angle [deg]') 71 | 72 | axl[2].set_xlabel('Sample') 73 | axr[2].set_xlabel('Sample') 74 | fl.suptitle('Left Hip Angles') 75 | fr.suptitle('Right Hip Angles') 76 | 77 | fl.tight_layout(rect=[0, 0.03, 1, 0.95]) 78 | fr.tight_layout(rect=[0, 0.03, 1, 0.95]) 79 | 80 | 81 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open("README.md", "r") as fh: 4 | long_description = fh.read() 5 | 6 | # these lines allow 1 file to control the version, so only 1 file needs to be updated per version change 7 | fid = open('pykinematics/version.py') 8 | vers = fid.readlines()[-1].split()[-1].strip("\"'") 9 | fid.close() 10 | 11 | setuptools.setup( 12 | name="pykinematics", 13 | version=vers, 14 | author="Lukas Adamowicz", 15 | author_email="lukas.adamowicz95@gmail.com", 16 | description="Calculation of hip joint angles from wearable inertial sensors and optical motion capture.", 17 | long_description=long_description, 18 | long_description_content_type="text/markdown", 19 | url="https://github.com/M-SenseResearchGroup/pymotion", # project url, most likely a github link 20 | # download_url="https://github.com/M-SenseResearchGroup/pymotion", # link to where the package can be downloaded, most likely PyPI 21 | project_urls={ 22 | "Documentation": "https://pykinematics.readthedocs.io/en/latest/" 23 | }, 24 | include_pacakge_data=False, # set to True if you have data to package, ie models or similar 25 | # package_data={'pykinematics.imu.data': ['*.h5']}, 26 | # package_data={'package.module': ['data.csv']}, # if data.csv is in a separate module 27 | packages=setuptools.find_packages(), # automatically find required packages 28 | license='MIT', 29 | python_requires='>=3.6', # Version of python required 30 | install_requires=[], 31 | classifiers=[ 32 | "Programming Language :: Python :: 3", 33 | "License :: OSI Approved :: MIT License", 34 | "Operating System :: OS Independent", 35 | "Topic :: Scientific/Engineering", 36 | "Programming Language :: Python :: 3.7" 37 | ], 38 | ) 39 | --------------------------------------------------------------------------------