From other experiments we suspected that there is a difference in the clock speed between qualisys and the treadmill clock. We verified this using a Fourier-based approach:
Approach:
In [2]:
%connect_info
In [4]:
import scipy.io as sio
import mutils.misc as mi
import mutils.io as mio
import mutils.FDatAn as fda
reload(fda)
#s = 7 # subject id
#r = 1 # repetition
import sys
for s in range(10):
print "subject", s
for r in range(7):
for t in range(8):
fn_kin = 'data/2011-mmcl_mat/subj%i/kin/kin_t%i_r%i.mat' % (s, t, r)
fn_frc = 'data/2011-mmcl_mat/subj%i/frc/frc_t%i_r%i.mat' % (s, t, r)
try:
kd = sio.loadmat(fn_kin)
fd = sio.loadmat(fn_frc)
except IOError:
sys.stdout.write('x')
continue
sys.stdout.write('.')
#sk, sf = [mio.saveable(x) for x in [kd, fd]] # convenient access
fc, kc = fda.ComputeCoM(kd,fd, use_Fint=True, adapt_kin_mean=True)
kd['com'] = kc[1::4,:]
fd['fx_c'] = fc[:,0]
fd['fy_c'] = fc[:,1]
fd['fz_c'] = fc[:,2]
# remove "old" matlab keys from dict
_ = [kd.pop(key) for key in ['__header__', '__globals__', '__version__'] ]
_ = [fd.pop(key) for key in ['__header__', '__globals__', '__version__'] ]
sio.savemat(fn_kin, kd, do_compression=True)
sio.savemat(fn_frc, fd, do_compression=True)
print "done!"
In [2]:
import scipy
import scipy.fftpack as f
import numpy as np
def kin_estimate(selectedData):
"""
calculates the kinematic CoM estimate from "selectedData"
"""
# anthropometry from Dempster
# format: prox.marker|dist. marker| rel. weight (%) | segment CoM pos rel. to prox. marker (%)
aData = [
('R_Hea','L_Hea',8.26,50.),
('L_Acr','R_Trc',46.84/2.,63.),
('R_Acr','L_Trc',46.84/2.,63.),
('R_Acr','R_Elb',3.25,43.6),
('R_Elb','R_WrL',1.87 + 0.65,43. + 25.),
('L_Acr','L_Elb',3.25,43.6),
('L_Elb','L_WrL',1.87 + 0.65,43. + 25.),
('R_Trc','R_Kne',10.5,43.3),
('R_Kne','R_AnL',4.75,43.4),
('R_Hee','R_Mtv',1.43,50. + 5.),
('L_Trc','L_Kne',10.5,43.3),
('L_Kne','L_AnL',4.75,43.4),
('L_Hee','L_Mtv',1.43,50. + 5.)
]
# adaptation to dataformat when extracted from database
aData = [(x[0].lower(), x[1].lower(), x[2], x[3]) for x in aData]
CoM = np.zeros((len(selectedData['sacr'][:,0]),3))
for segment in aData:
CoM += segment[2]/100.* (
(selectedData[segment[1]]-selectedData[segment[0]])*segment[3]/100.
+ selectedData[segment[0]]
)
return CoM
def CoM_combine(CoM,Force,fs,mass,f_thresh = 2.0,s = 10.):
"""
performs the CoM_combination
parameters:
CoM 1-D array! kinematic CoM estimate
Force 1-D array! force in N (must be mean corrected)
fs sampling frequency
mass mass in kg
f_thresh threshold frequency, default: 2.0 Hz
s steepness of threshold, usually ~ 10
returns: new (CoM, Force)
"""
# first: define two helper functions, "weighting_function" and "kin_estimate"
def weighting_function(freq_vec, f_changeover, sharpness):
"""
returns a weighting function to be multiplied with the spectra
"""
weighting = 1./( 1. + np.exp( ( freq_vec[1:] - f_changeover ) * sharpness ) );
weighting = weighting + weighting[::-1]
weighting = np.concatenate( (np.array([1.]),weighting) )
return weighting
Force2 = (np.concatenate( (np.array([0.]),Force) )+\
np.concatenate( (Force,np.array([0.])) ))*0.5
vel_int = scipy.integrate.cumtrapz(Force2/mass,dx=1./fs)
spect_int = f.fft(vel_int)
spect_diff = f.fft( np.gradient(CoM))*fs
freq_vec = np.linspace(0,fs-1./len(Force),len(Force))
wv = weighting_function(freq_vec, f_thresh, s)
spect_combine = wv*spect_diff + (1.-wv)*spect_int;
v_combine = f.ifft(spect_combine).real
c_Force = np.gradient(v_combine)*fs*mass
v_combine = np.concatenate( (v_combine,np.array([0.])) )
c_CoM = scipy.integrate.cumtrapz(v_combine,dx = 1./fs) + CoM[0]
return (c_CoM,c_Force)
def calculate_CoM(selectedData,f_thresh = 2., s = 10.):
"""
calcualtes the CoM trajectory, based on (a) an anthropometric marker set,
and (b) on the CoM_combine method
parameters:
selectedData: data containing the required variables
f_thresh: threshold frequency
s: steepness
"""
mCoM = kin_estimate(selectedData)
Fz = selectedData['Fz'][:,0]
Fz -= np.mean(Fz)
Fy = selectedData['Fy'][:,0]
Fy -= np.mean(Fy)
CoM = np.zeros((len(Fz),3))
Force = np.zeros((len(Fz),3))
CoM[:,1], Force[:,1] = CoM_combine(mCoM[:,1],Fy,selectedData['fs'],selectedData['mass'])
CoM[:,2], Force[:,2] = CoM_combine(mCoM[:,2],Fz,selectedData['fs'],selectedData['mass'])
Force[:,2] += selectedData['mass']*9.81
# do not apply for Fx, because Fx is terribly wrong
CoM[:,0], Force[:,0] = mCoM[:,0],selectedData['Fx'][:,0]
selectedData['kinCoM'] = mCoM
selectedData['CoM'] = CoM
selectedData['Force'] = Force
In [3]:
import numpy as np
def w1(freq_vec, f_changeover, sharpness):
"""
returns a weighting function to be multiplied with the spectra
"""
weighting = 1./( 1. + np.exp( ( freq_vec[1:] - f_changeover ) * sharpness ) );
weighting = weighting + weighting[::-1]
weighting = np.concatenate( (np.array([1.]),weighting) )
return weighting
def w2(freq_vec, f_changeover, sharpness):
""" another weighting function, using tanh to prevent exp. overflow """
weight = .5 - .5*tanh((freq_vec[1:] - f_changeover) * sharpness)
weight = weight + weight[::-1]
weight = np.hstack([1., weight])
return weight
In [7]:
freq_vec=linspace(0,250., 4000, endpoint=False)
wa = w1(freq_vec, 5., 10.)
wb = w2(freq_vec, 5., 10.)
figure()
plot(freq_vec, wa,'r.', label='orig')
plot(freq_vec, wb,'b.-', label='tanh')
show()
In [7]:
from scipy.integrate import cumtrapz
kin_est = kin_estimate(kd)
Fz = [getattr(sf, key) for key in ['fzr' + str(x) for x in arange(4) + 1] + ['fzl' + str(x) for x in arange(4) + 1]]
Fz = sum(vstack(Fz), axis=0)
idx0a = 0
while Fz[idx0a] > 300:
idx0a += 1
idx0 = idx0a
while Fz[idx0] < 500:
idx0 += 1
idxE0 = 1
while Fz[-idxE0] < 800:
idxE0 += 1
idxE = idxE0
while Fz[-idxE] > 500:
idxE += 1
mass = mean(Fz[idx0:-idxE]) / 9.81
print "mass:", mass
vk = gradient(kin_est[:,2]) * 250.
vf = cumtrapz(Fz - mass*9.81, initial=vk[0]) / 1000.
tk = linspace(0, 240, vk.shape[0], endpoint=False)
tf = linspace(0, 240, vf.shape[0], endpoint=False)
vki = interp(tf, tk, vk)
# manually extrapolate last 3 points
delta = vki[-4] - vki[-5]
for idx in arange(3) + 1:
vki[-idx] += (4 - idx) * delta
In [7]:
slices = [slice(0,30000), slice(70000,100000), slice(140000, 170000), slice(210000,240000)] # equidistant
sdk = [f.fft(vki[sl]) for sl in slices]
sdf = [f.fft(vf[sl]) for sl in slices]
# frequency ~ 2.8Hz:
freq_vec = linspace(0,1000, 30000, endpoint=False)
figure()
sel = slice(70,95) # frequencies between 2.5 and 3.1 Hz
idx = 0
for s1, s2 in zip(sdk, sdf):
idx += 1
plot(freq_vec[sel], angle(s1[sel] / s2[sel]), '.', label=('part %i' % idx))
xlabel('frequency [Hz]')
fmax = argmax(abs(s1[sel]))
plot([freq_vec[sel][fmax],] * 2, [-1,1],'k--')
gca().set_ylim(-.5, .5)
title('phase differences (black dashes: dominant frequency)')
legend(loc='best')
Out[7]:
if we assume that one estimate $a'$ is related to the other $a$ by $a'(t) = a(t + \Delta t)$ , then we can compute their spectra and find the relation of the spectra:
$\tilde{A}'(\omega) = \tilde{A} e^{2\pi i \omega \Delta t}$
and from this we can calulate $\Delta t$ by:
and
$\angle \frac{\tilde{A}'}{\tilde{A}} = \tan^{-1} (2\pi \omega \Delta t)$
or
$\Delta t = \angle \frac{\tilde{A}'}{\tilde{A}} \frac{1}{2 \pi \omega}$
In [8]:
phis = [arctan(angle(s1[sel][fmax] / s2[sel][fmax])) for s1, s2 in zip(sdk, sdf)]
freq_max = freq_vec[sel][fmax]
delta_t = [phi / (2*pi*freq_max) for phi in phis]
figure()
plot(delta_t, 'r.', label='from exp.')
p = polyfit(range(len(delta_t)), delta_t, 1)
q = polyval(p, [0, len(delta_t) - 1])
plot([0, len(delta_t) - 1], q, 'b--', label='fit')
title('time shift as function of time\ntotal difference: %1.2e' % (q[1] - q[0]))
legend(loc='best')
xlabel('investigated part')
Out[8]:
In [18]:
comzi = interp(tf, tk, kin_est[:,2])
tf_2 = linspace(0, 240.012, vf.shape[0], endpoint=False)
Fzi = interp(tf, tf_2, Fz)
delta = comzi[-4] - comzi[-5]
# linearly extrapolate last three indices
for elem in range(3):
comzi[-(1+elem)] += (3-elem)*delta
In [19]:
zc, Fzc = CoM_combine(comzi, Fz - 9.81*mass, 1000, mass,f_thresh = 2.0,s = 10.)
zc2, Fzc2 = CoM_combine(comzi, Fzi - 9.81*mass, 1000, mass,f_thresh = 2.0,s = 10.)
In [11]:
figure()
plot(Fzc + 9.81*mass, 'r.-')
plot(Fz , 'g.-')
xlim(0,1000)
Out[11]:
In [21]:
figure()
subplot(1,2,1)
plot(zc,'g+')
plot(comzi,'r.')
plot(zc2,'b.')
subplot(1,2,2)
plot(zc,'g+')
plot(comzi,'r.')
plot(zc2,'b.')
Out[21]:
In [23]:
figure()
subplot(1,2,1)
plot(Fzc + mass*9.81,'g+')
plot(Fz,'r.')
plot(Fzc2+ mass*9.81,'b.')
subplot(1,2,2)
plot(Fzc+ mass*9.81,'g+')
plot(Fz,'r.')
plot(Fzc2+ mass*9.81,'b.')
Out[23]:
In [20]:
close('all')
In [ ]: