rotmat.py:300: RuntimeWarning: invalid value encountered in sqrt q[3] = 0.5 * np.copysign(np.sqrt(1-R11-R22+R33), R21-R12)

This is the data in question: skinematics-invalid-sqrt.txt

running a unittest on my local machine, I ran into this assertion:

FAIL: test_kalman (skinematics.tests.test_imus.TestSequenceFunctions)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "C:\...\scikit-kinematics\skinematics\tests\test_imus.py", line 87, in test_kalman
    self.assertAlmostEqual(error, 0, places=2)  # It is not clear why the Kalman filter is not more accurate
AssertionError: 0.20809466821986328 != 0 within 2 places (0.20809466821986328 difference)

----------------------------------------------------------------------
Ran 49 tests in 6.549s

FAILED (failures=1)

My environment: Windows 10, Python 3.8.6,
alabaster==0.7.12
attrs==19.3.0
Babel==2.8.0
backcall==0.2.0
bleach==3.1.5
CacheControl==0.12.6
cachetools==4.1.1
certifi==2020.6.20
cffi==1.14.2
chardet==3.0.4
colorama==0.4.3
cryptography==3.0
cycler==0.10.0
decorator==4.4.2
defusedxml==0.6.0
docutils==0.16
entrypoints==0.3
...
grpcio==1.32.0
httplib2==0.18.1
idna==2.10
imagesize==1.2.0
ipykernel==5.3.3
ipython==7.16.1
ipython-genutils==0.2.0
ipywidgets==7.5.1
jedi==0.17.2
Jinja2==2.11.2
jsonschema==3.2.0
jupyter==1.0.0
jupyter-client==6.1.6
jupyter-console==6.1.0
jupyter-core==4.6.3
jwcrypto==0.7
kaleido==0.0.1
kiwisolver==1.3.0
MarkupSafe==1.1.1
matplotlib==3.3.2
mistune==0.8.4
mpmath==1.1.0
msgpack==1.0.0
Naked==0.1.31
nbconvert==5.6.1
nbformat==5.0.7
notebook==6.0.3
numpy==1.19.1
oauth2client==4.1.3
packaging==20.4
pandas==1.0.5
pandocfilters==1.4.2
parso==0.7.0
pickleshare==0.7.5
pika==1.1.0
Pillow==8.0.1
plotly==4.9.0
prometheus-client==0.8.0
prompt-toolkit==3.0.5
protobuf==3.13.0
pyasn1==0.4.8
pyasn1-modules==0.2.8
pycparser==2.20
pygame==2.0.0
Pygments==2.6.1
PyOpenGL==3.1.5
pyparsing==2.4.7
pyrsistent==0.16.0
python-dateutil==2.8.1
python-jwt==3.2.6
pytz==2020.1
pywin32==228
pywinpty==0.5.7
PyYAML==5.3.1
pyzmq==19.0.1
qtconsole==4.7.5
QtPy==1.9.0
requests==2.24.0
requests-toolbelt==0.9.1
retrying==1.3.3
rsa==4.6
scikit-kinematics==0.8.5
scipy==1.5.3
Send2Trash==1.5.0
shellescape==3.8.1
six==1.15.0
snowballstemmer==2.0.0
Sphinx==3.2.1
sphinxcontrib-applehelp==1.0.2
sphinxcontrib-devhelp==1.0.2
sphinxcontrib-htmlhelp==1.0.3
sphinxcontrib-jsmath==1.0.1
sphinxcontrib-qthelp==1.0.3
sphinxcontrib-serializinghtml==1.1.4
sseclient==0.0.26
sympy==1.6.2
terminado==0.8.3
testpath==0.4.4
tornado==6.0.4
traitlets==4.3.3
uritemplate==3.0.1
urllib3==1.25.10
wcwidth==0.2.5
webencodings==0.5.1
widgetsnbextension==3.5.1

Hello Thomas:

Nice library. The docs of Madgwick and Mahony classes are probably missing the measurement units to be passed. I'll be happy to contribute to the library docs if you let me know here.

Thanks,
MTA.

I've got binary datafiles from Xens (.mvn) but it seems the code is setup to work with a different kind of file (a .csv?). Maybe the format has changed, or maybe I need to preprocess it first somehow?

There seems to be an inconsistency with the use of the sample rate when comparing the Kalman filter to the Madgwick filter.

The Kalman filter takes a rate in Hz as input (which is later converted into seconds) while the Madgwick filter takes a sample period in seconds. This makes it inconvenient to use the same input_data for both filters.

Is it possible to adjust this issue? I'm also ready to submit a PR if needed.

You used to have drift compensation in imus.py, line382, which equals 0.7*mean(acc). If I have mean=-10.02 m/s^2 in my data (vertical axis), does it mean I have an uncalibrated accelerometer and I need to use drift compensation then?

The doc states that input for Madgwick.Update and Mahony.Update can be numpy arrays of shape (N,3).

I don't think this works as both functions use the line

np.hstack([0, Gyroscope])

which throws

ValueError: all the input arrays must have same number of dimensions

analytical() rotates the initial quat according to R_initialOrientation. Kalman, Madgwick and Mahony all appear to initialize to zero quat-vector. Is it possible to initialize these 3 functions analogously to analytical function?

imus.py, line 358 - g is hard-coded in 3rd position, while MEMS accelerometer has vertical axis on Y, positive upwards.

imus.py, line 357 - g constant is hard-coded to 2nd digit, can be taken from scipy.constants.g

N.B. I have tested this using a manual sensor with data gathered previously, both with and without magnetometer values (using Madgwick and Analytical, respectively).

EDIT: Based on further review, I believe that R_init is not ignored entirely, but valid rotation matricies don't produce the desired effect (see below). I have therefore removed some previous analysis in this post.

I am new to IMUs and kinematics in general, so if I have made some error, I deeply apologise.
Thank you for your time.

Hi Thomas,

if I try to set sample rate, acceleration and gyroscope data by the in_data parameter of the IMU_Base constructor I get an error. The error is caused by the function call self._set_info() in line 199 of imus.py (last call in _set_data that is called with in_data in function __init__). But the function expects all parameters in its definition (def _set_info(self, rate, acc, omega, mag): line 295 of imus.py). I solved it for me in the rather crude way to call it with
self._set_info(rate = self.rate, acc = self.acc, omega = self.omega, mag = self.mag) from _set_data (line 199).

Besides that I have a question regarding the unit of the gyroscope data. In the comment for the IMU_Base class (line 65) the unit is mentioned as [deg/s] but in nearly all other cases as [rad/s] - as far as I've seen from a first look. I also didn't noticed a conversion between rad/s and deg/s. That's why I suppose the gyroscope data should be provided in [rad/s]. Is this correct?

Best regards,
dj

I just installed this package on a new computer using pip install scikit-kinematics. When I try to import skinematics I get an error that says ImportError: No module named 'sympy'

I resolved this issue by manually installing sympy, but I shouldn't have to do that. pip should automatically install sympy and all the other dependencies if necessary.

Upon trying to install and test skinematics, I was getting the following error:

>>> import skinematics as skin
Failed to import optional module imus. Install optional dependencies
Failed to import optional module misc. Install optional dependencies
>>>

I edited __init.py__ to print the full error and discovered that tkinter was not installed. This commonly occurs on linux distributions where the system version of python does not come with tkinter (which is usually istalled by default). The solution for this is sudo apt install python3-tk or the equivalent command for your linux distro.

Hi,

I installed scikit-kinematics on my MacOS X system via pip3 (pip3 install scikit-kinematics), installation went smoothly, but when I try to import library it throws an error:

$ python3
Python 3.7.2 (default, Feb 12 2019, 08:15:36)
[Clang 10.0.0 (clang-1000.11.45.5)] on darwin
Type "help", "copyright", "credits" or "license" for more information.
>>> import skinematics as skin
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/usr/local/lib/python3.7/site-packages/skinematics/__init__.py", line 29, in <module>
    importlib.import_module('.'+_m, package='skinematics')
  File "/usr/local/Cellar/python/3.7.2_2/Frameworks/Python.framework/Versions/3.7/lib/python3.7/importlib/__init__.py", line 127, in import_module
    return _bootstrap._gcd_import(name[level:], package, level)
  File "/usr/local/lib/python3.7/site-packages/skinematics/view.py", line 34, in <module>
    from matplotlib.mlab import dist
ImportError: cannot import name 'dist' from 'matplotlib.mlab' (/usr/local/lib/python3.7/site-packages/matplotlib/mlab.py)
>>>

Any suggestion what can be wrong with my setup?
Thanks in advance.

Example:

import skinematics as skin
q = skin.quat.Quaternion([1., 0., 0., 0.])
q.export('euler') # returns none, expected [0., 0., 0.]

Quaternion=[1, 0, 0, 0] argument of madgwick filter effectively rotates the data 90 degress around X axis:

from skinematics.sensors.xsens import XSens
from skinematics.view import Orientation_OGL


in_file = r'd:\myProject\venv\Lib\site-packages\skinematics\tests\data\data_xsens.txt'

sensor = XSens(in_file=in_file, q_type='madgwick')


viewer=Orientation_OGL(quat_in=sensor.quat)
viewer.run()

Animation is not shown if video file is None.
Animation in video file has the whole figure scrolling bottom-up while being animated inside.

Can I know the real motion of the test data? (specially with the XIO sensor)
I tested some data with the library (not the data of the repository) and the results are not a little bit similar with the real motion, maybe I am using wrong the library. That's why I'am asking for the real motion of the test data, in order to know what should I expect when I try the library. Thanks.

It would be tremendously helpful if the library could be used only for the calculations, i.e. without OpenGL.

Currently, when we want to use from skinematics.imus import IMU_Base, then the __init__.py is called, which triggers the import of view (which in turn imports OpenGL)

Perhaps there could be an if statement that checks if OpenGL is installed.

Please let me know if you would like me to provide a solution in the form of a PR

v.0.6.8 view.orientation seems to cache the whole data when dealing with large datasets, thus yielding a long delay before showing the Tk window

Hello

I'm very interesting to use this library in my daily work, but I´m having some problems to adapt it.

I have seen that the function calc_position computes velocity and position.

1- How I can calculate the distance travelled by a user from the list of positions calculated?
2- How I can obtain the specific velocity value in each time step instead of the accumulative value?

Also, I have noted that when "Kalman" option is provided, the operation np.deg2rad is automatically done, How come? What if my data is initially in rad/seg?

Best regards

http://work.thaslwanter.at/skinematics/html/view.html

q = skin.quat.vel2quat(omegas, q0, rate, 'sf')

vel2quat seems to be deprecated, should be calc_quat

Hello.
If I run code from this example
"# Set the in-file, initial sensor orientation
in_file = r'tests/data/data_xsens.txt'
initial_orientation = np.array([[1,0,0],
[0,0,-1],
[0,1,0]])

Choose a sensor

from skinematics.sensors.xsens import XSens
..."
I get an error:
File "/usr/local/lib/python3.5/dist-packages/skinematics/sensors/xsens.py", line 16, in
from imus import IMU_Base
ImportError: No module named 'imus'

Checked in Ubuntu and Win8 - same error

Is it possible to use this library for real-time analysis? According to the documentation, the library requires recorded data. I want to use this library while streaming data from my IMU (BNO055 usb stick)

I understand little about sensor fusion. But I made the following test script:

from skinematics import imus, quat, vector, rotmat
from skinematics.simulations.simulate_movements import simulate_imu
import numpy as np
from skinematics.sensors.manual import MyOwnSensor

# Simulate IMU-data
duration_movement = 1  # [sec]
duration_total = 1 # [sec]
rate = 100  # [Hz]
direction = [1, 0.5, 0.25]
distance = 1
imu, body = simulate_imu(rate, duration_movement, duration_total, direction=direction, distance=distance)

in_data = {'rate': rate,
        'acc': imu['gia'],
        'omega':imu['omega'],
        'mag':imu['magnetic']
           }
np.set_printoptions(precision=3, suppress=True)
print("actual pos")
print(body['pos'][-1])
print("actual quat")
print(body['quat'][-1])


def test(q_type):
    my_sensor = MyOwnSensor(in_data=in_data, q_type=q_type, calculate_position=True)
    print(q_type + " pos")
    print(my_sensor.pos[-1])
    print(q_type + " quat")
    print(my_sensor.quat[-1])


for q_type in ['analytical', 'kalman', 'madgwick', 'mahony']:
    test(q_type)

I get these results:

actual pos
[0.873 0.436 0.218]
actual quat
[1. 0. 0. 0.]
analytical pos
[0.873 0.436 0.218]
analytical quat
[1. 0. 0. 0.]
kalman pos
[ 0.039  0.029 -4.414]
kalman quat
[ 1.    -0.013  0.013  0.015]
Calculating the Quaternions [#########################] 100/100
madgwick pos
[-0.059  0.014 -4.422]
madgwick quat
[ 0.965  0.004 -0.259  0.034]
Calculating the Quaternions [#########################] 100/100
mahony pos
[-0.011  0.015 -4.42 ]
mahony quat
[ 0.989 -0.004 -0.15   0.001]

All the methods apart from analytical have the position completely wrong despite getting the quaternion mostly right.

If I change the duration so that there's a stationary period after the movement then the results seem like a disaster. Even the analytical method starts making a mess of the position and the other methods are hopelessly wrong even about the quaternion.

Hi, all the current algorithms in module imus require magnetometer data available there. It would be great if magnetometer is optional when the magnetic inference is a big issue in some applications.

Thanks.

the original Madgwick and Mahony filter are available in 6DoF and 9DoF versions (http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/). It would be great if the project enabled the use of 6DoF as well. In fact, I need them in such versions.

analytical() integrates vel & pos using same code as calc_position() BUT after rotating the accelerometer data, it makes q[0] the "reference position". The other 3 orientation functions do not integrate vel & pos. Suggest harmonize analytical() with the others by eliminating the vel & pos integration. Then user would call calc_position() to recalculate.
Rotating q[0] to reference pos (and applying same rot to all of the q array) loses the initial pose calculation. Please extract this into a separate function that is available to user.

HI I'm totally new to IMUs and motion tracking but I'm looking for some python code to take some IMU data and estimate position during a simple kinematics experiment.

This seems like a promising project to help me but I"m not getting very good positions returned with my data. basically my positions seem to diverge exponentially, somewhat similarly to #37 and possibly #36. I think my issue might be do to not correcting for bias in the accelerometer and was wonder if there is a way to estimate/correct for this bias?

I also wonder if this project being developed? If not, is there a stable code base out there that people are using?

Thanks
Luke

I freshly installed scikit-kinematics, version 0.6.3, with pip3 into a Python 3.4.3 installation on Ubuntu 14.04 x64 LTS. When I try to import skinematics or any module included, I get the following:

Traceback (most recent call last):
  File "<input>", line 1, in <module>
  File "/usr/local/lib/python3.4/dist-packages/skinematics/__init__.py", line 29, in <module>
    importlib.import_module('.'+_m, package='skinematics')
  File "/usr/lib/python3.4/importlib/__init__.py", line 109, in import_module
    return _bootstrap._gcd_import(name[level:], package, level)
  File "/usr/local/lib/python3.4/dist-packages/skinematics/imus.py", line 31, in <module>
    from skinematics import quat, vector, misc, rotmat
  File "/usr/local/lib/python3.4/dist-packages/skinematics/quat.py", line 28, in <module>
    from skinematics import vector, rotmat
  File "/usr/local/lib/python3.4/dist-packages/skinematics/vector.py", line 24, in <module>
    from skinematics import quat
ImportError: cannot import name 'quat'

I think this is due to the fact that vector.py imports the quat module as well, which in turn is imported when the module is loaded.

according to my understanding, not quite understand the implementation of the Kalman:
(1)
...
z_k_pre = z_k.copy()
...
//# Update state vector x_k
x_k += np.array( K_k.dot(z_k-z_k_pre) ).ravel()
here z_k - z_k_pre means the current observation subtract the previous observation. according to standard Kalman text book, shouldn't here be observation subtract prediction ? and why the prediction part is just simply set to the previous observation (z_k_pre = z_k.copy())
(2)
....
x_k[:3] = np.zeros(3)
x_k[:3] += tstep * (-x_k[:3]+z_k[:3])
....
here the first 3 elements is forced to 0. so actually it is
x_k[:3] += tstep * (z_k[:3]),
since z_k[:3] is the angular velocity, the result is the amount of changed angles.

any help?

Tommy, I think it would be better to implement it with a "spatial transformation matrix" , as

import skinematics as skin
def stm(axis, angle, translation):
    """Spatial Transformation Matrix"""
    stm = np.diag([0,0,0,1.0])
    stm[:-1,:-1] = skin.rotmat.R(axis, angle)
    stm[:3,-1] = translation
    return stm

Unable to use scikit-kinematics package under Raspbian Buster:
Linux raspbari10 4.19.58-v7+ #1245 SMP Fri Jul 12 17:25:51 BST 2019 armv7l GNU/Linux
Installation via pip3 (19.2.1) or setup.py (python 3.7.3) worked fine without any error/warning messages.

Unfortunately, importing module into a python3 environment failed with following stream of messages (apologies for the length):

>>> import skinematics as skin
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/home/pi/projects/scikit-kinematics/skinematics/__init__.py", line 29, in <module>
    importlib.import_module('.'+_m, package='skinematics')
  File "/usr/lib/python3.7/importlib/__init__.py", line 127, in import_module
    return _bootstrap._gcd_import(name[level:], package, level)
  File "/home/pi/projects/scikit-kinematics/skinematics/imus.py", line 28, in <module>
    from scipy.integrate import cumtrapz
  File "/usr/local/lib/python3.7/dist-packages/scipy/integrate/__init__.py", line 89, in <module>
    from .quadrature import *
  File "/usr/local/lib/python3.7/dist-packages/scipy/integrate/quadrature.py", line 13, in <module>
    from scipy.special import roots_legendre
  File "/usr/local/lib/python3.7/dist-packages/scipy/special/__init__.py", line 641, in <module>
    from ._ufuncs import *
ImportError: libf77blas.so.3: cannot open shared object file: No such file or directory
>>> 

I did dutifully update all supporting modules/packages explicitly except BLAS because I did not have instructions for the latter. Thanks for looking into this.
Kind regards.

P.S.
I did revisit BLAS as follows:

pi@raspbari10:~/projects/scikit-kinematics $ sudo apt install libblas-dev liblapack-dev
Reading package lists... Done
Building dependency tree       
Reading state information... Done
libblas-dev is already the newest version (3.8.0-2).
libblas-dev set to manually installed.
liblapack-dev is already the newest version (3.8.0-2).
liblapack-dev set to manually installed.
0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.
pi@raspbari10:~/projects/scikit-kinematics $ 

Velocity cleaned from gravity component is calculated, but never stored. Could be useful.

Yun and Bachman used a legacy sensor combination for their paper, and gave appropriate empirical values for process noise variance D, gyro noise variance r_angvel and quaternion noise variance r_quats. The kalman() implementation hard-codes these parameters, as well as the arm movement time constant tau. Please make these tuning parameters more easily accessible: bring these up into the function call.