<html><head><meta name="color-scheme" content="light dark"></head><body><pre style="word-wrap: break-word; white-space: pre-wrap;">import xml.etree.ElementTree as ET
from lxml import etree

import matplotlib.pyplot as plt
import math
import os
import numpy as np
import tdsmParserMultis
import ConfigParser

def get_transformation_matrix(q1, q2, q3, q4, q5, q6):
    ''' Transform from optotrak global coordinates to optotrak position sensor coordinates'''

    T = np.zeros((4, 4))

    T[0, 0] = math.cos(q6) * math.cos(q5)
    T[1, 0] = math.sin(q6) * math.cos(q5)
    T[2, 0] = -math.sin(q5)

    T[0, 1] = math.cos(q6) * math.sin(q5) * math.sin(q4) - math.sin(q6) * math.cos(q4)
    T[1, 1] = math.sin(q6) * math.sin(q5) * math.sin(q4) + math.cos(q6) * math.cos(q4)
    T[2, 1] = math.cos(q5) * math.sin(q4)

    T[0, 2] = math.cos(q6) * math.sin(q5) * math.cos(q4) + math.sin(q6) * math.sin(q4)
    T[1, 2] = math.sin(q6) * math.sin(q5) * math.cos(q4) - math.cos(q6) * math.sin(q4)
    T[2, 2] = math.cos(q5) * math.cos(q4)

    T[0, 3] = q1
    T[1, 3] = q2
    T[2, 3] = q3
    T[3, 3] = 1

    return T

def convertTOarray(data):

    data = data.replace("", '')
    data = data.split(" ")
    corrected_list = data[2:len(data)]
    corrected_list[-1] = corrected_list[-1][0:-1]
    data_float = map(float, corrected_list)

    return np.asarray(data_float)

def get_xyzrpw(T):
    x = T[0,3]
    y = T[1,3]
    z = T[2,3]
    w = math.atan2(T[1,0], T[0,0])
    p = math.atan2(-T[2,0], math.sqrt(T[2,1]**2+T[2,2]**2))
    r = math.atan2(T[2,1], T[2,2])
    return x, y, z, r, p, w

def remove_peaks(data, threshold):
    for i in range(1,len(data)-1):
        if abs(data[i-1]-data[i]) &gt; threshold and abs(data[i+1]-data[i]) &gt; threshold:
            data[i] = (data[i-1] + data[i+1])/2
            print i, "REMOVE PEAK"
    return data

fig1,( ax1, ax2, ax3) = plt.subplots(nrows=3, ncols=1, figsize=(18, 9.5))

# Define the trial that you want to plot (TDMS file)
filename = '/home/morrile2/Documents/MULTIS Data/MULTIS_invitro/CMULTIS007-1/Data/044_CMULTIS007-1_UL_PC_I-1.tdms'

# Extract force information from TDMS file
data = tdsmParserMultis.parseTDMSfile(filename)

# Grab the appropriate state file to extract transformation matrices
file = os.path.split(filename)[1]
config = ConfigParser.RawConfigParser()
config.read(os.path.join(os.path.dirname(os.path.join(os.path.split(filename)[0])), 'Configuration', file[0:-5] + '_State.cfg'))

# Get the bone that is associated with the trial in the filename
seg = filename[-14:-11]

if seg == 'UL_':
    bone = 'Femur'
elif seg == 'LL_':
    bone = 'Tibia'
elif seg == 'UA_':
    bone = 'Humerus'
elif seg == 'LA_':
    bone = 'Radius'

# Extract force data and plot Fz
Fx = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Fx'])
Fy = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Fy'])
Fz = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Fz'])
Mx = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Mx'])
My = np.array(data[u'State.6-DOF Load'][u'6-DOF Load My'])
Mz = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Mz'])

ax1.plot(Fz, 'b')

# Extract the time for the optotrak data collection (subtract the first component for the inherent time delay in data collection)
time_optotrak = data[u'Time'][u'Time']
time_optotrak -= time_optotrak[0]

# US Optotrak sensor to US tip coordinate system
T_USOS_US = config.get('Probe-' + bone + ' Position', 't_sensor2_rb2')
T_USOS_US = convertTOarray(T_USOS_US).reshape(4, -1)

# Bone Optotrak sensor to Bone coordinate system
T_BOS_B = config.get('Probe-' +bone + ' Position', 't_sensor1_rb1')
T_BOS_B = convertTOarray(T_BOS_B).reshape(4, -1)

# Bone Optotrak sensor data in global coordinate system
q1 = data[u'Sensor.' + bone][u'' + bone + '_smart_02.x'] / 1000
q2 = data[u'Sensor.' + bone][u'' + bone + '_smart_02.y'] / 1000
q3 = data[u'Sensor.' + bone][u'' + bone + '_smart_02.z'] / 1000
q4 = np.radians(data[u'Sensor.' + bone][u'' + bone + '_smart_02.r'])
q5 = np.radians(data[u'Sensor.' + bone][u'' + bone + '_smart_02.p'])
q6 = np.radians(data[u'Sensor.' + bone][u'' + bone + '_smart_02.w'])

# Get raw ultrasound position (USOS) (TDMS) in global coordinate system
x = np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.x']) / 1000
y = np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.y']) / 1000
z = np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.z']) / 1000
r = np.radians(np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.r']))
p = np.radians(np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.p']))
w = np.radians(np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.w']))

# State data - US probe positions in bone coordinate system
state_x = np.array(data[u'State.Probe-'+ bone + ' Position'][u'Probe-'+ bone + ' Position x']) / 1000
state_y = np.array(data[u'State.Probe-'+ bone + ' Position'][u'Probe-'+ bone + ' Position y']) / 1000
state_z = np.array(data[u'State.Probe-'+ bone + ' Position'][u'Probe-'+ bone + ' Position z']) / 1000
state_r = np.radians(np.array(data[u'State.Probe-'+ bone + ' Position'][u'Probe-'+ bone + ' Position roll']))
state_p = np.radians(np.array(data[u'State.Probe-'+ bone + ' Position'][u'Probe-'+ bone + ' Position pitch']))
state_w = np.radians(np.array(data[u'State.Probe-'+ bone + ' Position'][u'Probe-'+ bone + ' Position yaw']))

pos_x = []
pos_y = []
pos_z = []
pos_r = []
pos_p = []
pos_w = []

#Find initial ultrasound tip coordinate system
ii = 0
T_W_USOS_i = get_transformation_matrix(x[ii], y[ii], z[ii], r[ii], p[ii], w[ii])
T_W_US_i = np.dot(T_W_USOS_i, T_USOS_US)

# Find the ultrasound position in the initial probe tip coordinate system
for ii in range(len(x)):
    T_B_US = get_transformation_matrix(state_x[ii], state_y[ii], state_z[ii], state_r[ii], state_p[ii], state_w[ii])
    T_W_BOS = get_transformation_matrix(q1[ii], q2[ii], q3[ii], q4[ii], q5[ii], q6[ii])

    T_W_USOS = get_transformation_matrix(x[ii], y[ii], z[ii], r[ii], p[ii], w[ii])
    T_W_US = np.dot(T_W_USOS, T_USOS_US)
    xi, yi, zi, ri, pi, wi = get_xyzrpw(np.dot(np.dot(np.dot(np.linalg.inv(T_W_US_i), T_W_BOS), T_BOS_B), T_B_US)) # US in US initial coordinate system
    # xi, yi, zi, ri, pi, wi = get_xyzrpw(np.dot(np.linalg.inv(T_W_US_i), T_W_US))
    pos_x.append(xi*1000)
    pos_y.append(yi*1000)
    pos_z.append(zi*1000)
    pos_r.append(ri)
    pos_p.append(pi)
    pos_w.append(wi)

# Remove the peaks in the data by averaging the point before and after the peak
print '++++++++++++++++++++++++++++++++'
pos_x = remove_peaks(pos_x, .5)
print '++++++++++++++++++++++++++++++++'
pos_y = remove_peaks(pos_y, .5)
print '++++++++++++++++++++++++++++++++'
pos_z = remove_peaks(pos_z, .5)
print '++++++++++++++++++++++++++++++++'

# Plot positions in mm (in reference to initial position of probe)
ax2.plot(time_optotrak, pos_x, 'r')
ax2.plot(time_optotrak, pos_y, 'g')
ax2.plot(time_optotrak, pos_z, 'b')

# Plot roll, pitch, yaw in degrees (in reference to initial position of probe)
ax3.plot(time_optotrak, np.degrees(pos_r), 'r')
ax3.plot(time_optotrak, np.degrees(pos_p), 'g')
ax3.plot(time_optotrak, np.degrees(pos_w), 'b')

# plt.figure()
# plt.plot(q1)
# plt.plot(q2)
# plt.plot(q3)

plt.show()


</pre></body></html>