def test_IMU_calc_orientation_position(self):
"""Currently, this only tests if the two functions are running through"""
# Get data, with a specified input from an XSens system
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
initial_orientation = np.array([[1,0,0],
[0,0,-1],
[0,1,0]])
initial_position = np.r_[0,0,0]
from skinematics.sensors.xsens import XSens
sensor = XSens(in_file=inFile, R_init=initial_orientation, pos_init=initial_position)
sensor.calc_position()
print('done')
def test_IMU_calc_orientation_position(self):
"""Currently, this only tests if the two functions are running through"""
# Get data, with a specified input from an XSens system
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
initial_orientation = np.array([[1,0,0],
[0,0,-1],
[0,1,0]])
initial_position = np.r_[0,0,0]
from skinematics.sensors.xsens import XSens
sensor = XSens(in_file=inFile, R_init=initial_orientation, pos_init=initial_position)
sensor.calc_position()
print('done')
def test_kalman(self):
# Analyze the simulated data with "kalman"
imu = self.imu_signals
q_kalman = imus.kalman(imu['rate'], imu['gia'], imu['omega'],
imu['magnetic'])
# and then check, if the quat_vector = [0, sin(45), 0]
result = quat.q_vector(q_kalman[-1]) # [0, 0.46, 0]
correct = array([0., np.sin(np.deg2rad(45)), 0.]) # [0, 0.71, 0]
error = norm(result - correct)
self.assertAlmostEqual(
error, 0, places=2
) # It is not clear why the Kalman filter is not more accurate
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0, 0, 0])
R_initialOrientation = rotmat.R(0, 90)
sensor = XSens(in_file=inFile,
R_init=R_initialOrientation,
pos_init=initialPosition,
q_type='kalman')
print(sensor.source)
q = sensor.quat
def test_mahony(self):
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0,0,0])
R_initialOrientation = rotmat.R1(90)
sensor = XSens(in_file=inFile, R_init = R_initialOrientation, pos_init = initialPosition, q_type='mahony')
q = sensor.quat
def test_set_qtype(self):
"""Tests if the test crashes on any of the existing qtype options"""
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0, 0, 0])
R_initialOrientation = rotmat.R(0, 90)
sensor = XSens(in_file=inFile,
R_init=R_initialOrientation,
pos_init=initialPosition,
q_type='kalman')
allowed_values = ['analytical', 'kalman', 'madgwick', 'mahony', None]
for sensor_type in allowed_values:
sensor.set_qtype(sensor_type)
print('{0} is running'.format(sensor_type))
def test_set_qtype(self):
"""Tests if the test crashes on any of the existing qtype options"""
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0,0,0])
R_initialOrientation = rotmat.R(0,90)
sensor = XSens(in_file=inFile, R_init = R_initialOrientation, pos_init = initialPosition, q_type='kalman')
allowed_values = ['analytical',
'kalman',
'madgwick',
'mahony',
None]
for sensor_type in allowed_values:
sensor.set_qtype(sensor_type)
print('{0} is running'.format(sensor_type))
def test_kalman(self):
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0, 0, 0])
R_initialOrientation = rotmat.R(0, 90)
sensor = XSens(in_file=inFile,
R_init=R_initialOrientation,
pos_init=initialPosition,
q_type='kalman')
print(sensor.source)
q = sensor.quat
def test_analytical(self):
# Get data
inFile = os.path.join(myPath, 'data', 'data_xsens.txt')
from skinematics.sensors.xsens import XSens
initialPosition = array([0,0,0])
R_initialOrientation = rotmat.R1(90)
sensor = XSens(in_file=inFile, R_init = R_initialOrientation, pos_init = initialPosition)
rate = sensor.rate
acc = sensor.acc
omega = sensor.omega
plt.plot(sensor.quat)
plt.show()
def get_data(file_path):
"""
Fetch data from provided path if valid and open a dialog window for path selection otherwise
:param file_path: path to the file with sensor data
:return: the data parsed as an XSense object
"""
in_file = file_path
# if no valid path was provided ...
if not Path(in_file).exists():
# ... get a valid path
in_file = filedialog.askopenfilename(
title="File not found in current folder. Please select file")
# read in data
data = XSens(in_file, q_type=None)
return data
def get_sensor_data(file):
"""
Returns acceleration and gyrodata recorded by IMU
"""
data = XSens(file, q_type=None)
return data.acc, data.omega
def task1():
"""Simulate the neural vestibular responses during walking:
Calculate the maximum cupular displacements (positive and negative)
and write them to CupularDisplacement.txt
Calculate the minimum and maximum acceleration along a given otolith direction
and write them to MaxAcceleration.txt
"""
#Read in the data
in_file = r'./MovementData/Walking_02.txt'
out_dir = r'./out'
try:
os.mkdir(out_dir)
print("Created directory ", out_dir)
except FileExistsError:
pass
sensor = XSens(in_file=in_file)
#Extract data from sensor
N = sensor.totalSamples
sample_rate = sensor.rate
# (N * 3) dimensional array of accelerations
accs = sensor.acc
# (N * 3) dimensional array of omegas
omegas = sensor.omega
### 1.
# Transform from head coordinates / world coords to Approximate IMU coords
#Equivalent to:
R_hc_approx = sk.rotmat.R(axis='x', angle=90)
g_hc = np.array([0, 0, -9.81])
g_approx = R_hc_approx @ g_hc # [0. , 9.81, 0. ]
### 2.
# Assume acceleration vector at time 0 is only gravity
g_IMU = accs[0] # [ 4.37424 8.578849 -1.814515]
# Define quaternion that defines the smallest rotation between g_approx and g_IMU
alpha = arccos((np.dot(g_approx, g_IMU)) / (norm(g_approx) * norm(g_IMU)))
m = cross(g_approx, g_IMU) / norm(cross(g_approx, g_IMU))
q_approx_IMU = sk.quat.Quaternion(
m * sin(alpha / 2)) # quaternion approx -> IMU
R_approx_IMU = q_approx_IMU.export(
'rotmat') #-> in case one wants to do the computations using matrices
### 3.
# Transformation from 'head coordinates' / 'world coordinates at t=0' to IMU coords
# Rotation matricies should be interpreted from right to left
# @ is matrix multiplication in numpy, * is elementwise
R_hc_IMU = R_approx_IMU @ R_hc_approx # transform hc -> approx -> IMU
R_rc_hc = sk.rotmat.R(
axis='y', angle=15) # Reid's line coords (rc) -> head coords (hc)
R_rc_IMU = R_hc_IMU @ R_rc_hc # rc -> hc -> IMU
# Semi circular canal vector in Reid's line coordinates
SCC_v = np.transpose(np.array([0.32269, -0.03837, -0.94573]))
# Otolith direction vectors
Otolith_dir_hc = np.transpose(np.array([0, 1, 0]))
### 4.
# Transform vectors to IMU coordinates
SCC_v_IMU = R_rc_IMU @ SCC_v
Otolith_dir_IMU = R_hc_IMU @ Otolith_dir_hc
### 5.
# SCC stimulation
# [Nx3] * [3x1] \in [Nx1] -> one value for every time step
SCC_stim_all = []
for i in range(N):
SCC_stim = np.dot(np.transpose(omegas[i]), SCC_v_IMU)
SCC_stim_all.append(SCC_stim)
# Otolith stimulation
# [Nx3] * [3x1] \in [Nx1] -> one value for every time step
Ot_stim_all = []
for i in range(N):
Ot_stim = np.dot(np.transpose(accs[i]), Otolith_dir_IMU)
Ot_stim_all.append(Ot_stim)
### 6.
# Cupula displacement for head movements
# SCC dynamics
T1 = 0.01
T2 = 5
# Define transfer function
num = [T1 * T2, 0]
den = [T1 * T2, T1 + T2, 1]
scc_transfer_function = signal.lti(num, den)
# Radius of SCC
radius = 3.2 #mm
# Create time axis with length and increments of sensor data
t = np.arange(0, 1. / sample_rate * N, 1. / sample_rate)
# Estimate displacement (radians) with calculated SCC_stim_all
_, out_sig, _ = signal.lsim(scc_transfer_function, SCC_stim_all, t)
# radians -> mm
cuppula_displacements = out_sig * radius
#For visualization
# plt.hist(cuppula_displacements, bins=100)
# plt.show()
max_pos = np.max(cuppula_displacements)
max_neg = np.min(cuppula_displacements)
print('Maximal positive cupular displacement:', max_pos, 'mm')
print('Maximal negative cupular displacement:', max_neg, 'mm')
with open(f'{out_dir}/CupularDisplacement.txt', 'w+') as f:
f.write(f'Maximal positive cupular displacement: {max_pos}\n')
f.write(f'Maximal negative cupular displacmenet: {max_neg}\n')
print('Wrote values to CupularDisplacement.txt')
### 7.
# Minimum / maximum acceleration along Ot_dir_IMU direction [m/s²]
# Projection of acceleration vector onto Ot_dir_IMU, then determine vector norm
# https://en.wikipedia.org/wiki/Vector_projection
# dir_acc(t) = dot(acc(t),Ot_dir_IMU) * Ot_dir_IMU
# max_t/min_t norm ( dir_acc(t) )
# Projectiong on a unit vector and taking the norm of that is equivalent to
# simply taking taking the dot product between the two.
# (same calculation as Ot_stim_all)
norms = Ot_stim_all
max_acc = np.max(norms)
min_acc = np.min(norms)
print('Maximal acceleration along otolith direction:', max_acc, 'm/s²')
print('Minimal acceleration along otolith direction:', min_acc, 'm/s²')
with open(f'{out_dir}/MaxAcceleration.txt', 'w+') as f:
f.write(
f'Maximal acceleration along otolith direction: {max_acc} m/s²\n')
f.write(
f'Minimal acceleration along otolith direction: {min_acc} m/s²\n')
print('Wrote values to MaxAcceleration.txt')
return R_hc_IMU
def task2(R_hc_IMU):
""" Calculate the orientation of the "Nose"-vector
Plot quaternion values
Plot quaternion vector values, save orientations to video
and output the orientation at the end of walking the loop
"""
out_video_file = './out/task2_out.mp4'
out_plot_file = "./out/task2_out.png"
R_IMU_hc = np.transpose(R_hc_IMU)
Nose_init_hc = np.transpose(np.array([1, 0, 0]))
#Read in sensor data
in_file = r'./MovementData/Walking_02.txt'
sensor = XSens(in_file=in_file)
N = sensor.totalSamples
sample_rate = sensor.rate
# (N * 3) dimensional array of omegas
omegas = sensor.omega
# Transform omegas to head coordinates
omegas_hc = []
for i in range(N):
omega_hc = R_IMU_hc @ np.transpose(omegas[i])
omegas_hc.append(np.transpose(omega_hc))
# (N * 3) dimensional array of omegas in head coordinates
omegas_hc = np.array(omegas_hc)
# Calculate all orientation quaternions
qs = -sk.quat.calc_quat(omegas_hc, [0, 0, 0], sample_rate, 'bf')
# Output of last orientation of nose
q_last = qs[-1, :]
R_last = sk.quat.convert(q_last)
Nose_end_hc = R_last @ Nose_init_hc
print('Nose orientation at the end of the walking the loop:', Nose_end_hc)
# Graph of all quaternion components
# Only plot vector part
plt.plot(range(N), qs[:, 1])
plt.plot(range(N), qs[:, 2])
plt.plot(range(N), qs[:, 3])
plt.savefig(out_plot_file)
print('Plot image saved to', out_plot_file)
plt.show()
# Create moving plot of nose vector
# Use scikit-kinematics visualizations
# (Need ffmpeg)
print('Creating animation of orientations...')
sk.view.orientation(qs,
out_video_file,
'Nose orientation',
deltaT=1000. / sample_rate)
# The easiest way to specify the approximate orientation is by indicating
# the approximate direction the$(x,y,z)$axes of the IMU are pointing at:
x = [1, 0, 0]
y = [0, 0, 1]
z = [0, -1, 0]
initial_orientation = np.column_stack((x, y, z))
initial_position = np.r_[0, 0, 0]
orientation_calculation = 'analytical' # Method for orientation calculation
# Reading in the data, and initializing the ``Sensor'' object. In this step also
# the orientation is calculated.
# To read in data from a different sensor, the corresponding class has to be
# imported from skinematics.sensors.
my_imu = XSens(in_file=data_file,
q_type=orientation_calculation,
R_init=initial_orientation,
pos_init=initial_position)
# Example 1: extract the raw gyroscope data
gyr = my_imu.omega
time = np.arange(my_imu.totalSamples) / my_imu.rate
# Set the graphics parameters
sns.set_context('poster')
sns.set_style('ticks')
# Plot it in the left figure
fig, axs = plt.subplots(1, 2, figsize=[18, 8])
lines = axs[0].plot(time, gyr)
axs[0].set(title='XSens-data',
transfer_data[param] = in_data[param]
except KeyError:
print('{0} is a required argument for manual data input!', param)
raise
if 'mag' in in_data.keys():
transfer_data['mag'] = in_data['mag']
self._set_data(transfer_data)
if __name__ == '__main__':
from skinematics.sensors.xsens import XSens
import matplotlib.pyplot as plt
xsens_sensor = XSens(in_file=r'..\tests\data\data_xsens.txt')
#xsens_sensor = XSens(in_file=r'..\tests\data\data_xsens.txt', q_type=None)
in_data = {
'rate': xsens_sensor.rate,
'acc': xsens_sensor.acc,
'omega': xsens_sensor.omega,
'mag': xsens_sensor.mag
}
my_sensor = MyOwnSensor(in_file='My own 123 sensor.', in_data=in_data)
print(my_sensor.omega[:3, :])
plt.plot(my_sensor.acc)
plt.show()
print('Done')
import matplotlib.pyplot as plt
import os
# Import skinematics
from skinematics.sensors.xsens import XSens
from skinematics.quat import Quaternion
# Get the data
data_dir = r'D:\Users\thomas\Coding\Python\scikit-kinematics\skinematics\tests\data'
infile_ll = os.path.join(data_dir, 'walking_xsens_lowerLeg.txt')
infile_ul = os.path.join(data_dir, 'walking_xsens_upperLeg.txt')
# Provide the approximate initial orientation of the IMUs
initial_orientation = np.array([[0, 0, -1], [1, 0, 0], [0, -1, 0]]).T
sensor_ul = XSens(infile_ul, R_init=initial_orientation)
sensor_ll = XSens(infile_ll, R_init=initial_orientation)
# Convert the orientation to quaternions
q_upperLeg = Quaternion(sensor_ul.quat)
q_lowerLeg = Quaternion(sensor_ll.quat)
'''
Calculate the 3-D knee orientation, using "Quaternion" objects
Using the two rules for combined rotations
* From right to left
* From the inside out
we get that the orientation of the
lower_leg = upper_leg * knee
Bringing the "upper_leg" to the other side, we have
knee = inv(upper_leg) * lower_leg
'''
