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 '''