def log_normal(name, pi, sigma, p, s):
""" Generate PyMC objects for a lognormal model
:Parameters:
- `name` : str
- `pi` : pymc.Node, expected values of rates
- `sigma` : pymc.Node, dispersion parameters of rates
- `p` : array, observed values of rates
- `s` : array, standard error sizes of rates
:Results:
- Returns dict of PyMC objects, including 'p_obs' and 'p_pred' the observed stochastic likelihood and data predicted stochastic
"""
assert pl.all(p > 0), "observed values must be positive"
assert pl.all(s >= 0), "standard error must be non-negative"
i_inf = pl.isinf(s)
@mc.observed(name="p_obs_%s" % name)
def p_obs(value=p, pi=pi, sigma=sigma, s=s):
return mc.normal_like(pl.log(value), pl.log(pi + 1.0e-9), 1.0 / (sigma ** 2.0 + (s / value) ** 2.0))
s_noninf = s.copy()
s_noninf[i_inf] = 0.0
@mc.deterministic(name="p_pred_%s" % name)
def p_pred(pi=pi, sigma=sigma, s=s_noninf):
return pl.exp(mc.rnormal(pl.log(pi + 1.0e-9), 1.0 / (sigma ** 2.0 + (s / (pi + 1.0e-9)) ** 2)))
return dict(p_obs=p_obs, p_pred=p_pred)
def offset_log_normal(name, pi, sigma, p, s):
""" Generate PyMC objects for an offset log-normal model
:Parameters:
- `name` : str
- `pi` : pymc.Node, expected values of rates
- `sigma` : pymc.Node, dispersion parameters of rates
- `p` : array, observed values of rates
- `s` : array, standard error sizes of rates
:Results:
- Returns dict of PyMC objects, including 'p_obs' and 'p_pred' the observed stochastic likelihood and data predicted stochastic
"""
assert pl.all(p >= 0), 'observed values must be non-negative'
assert pl.all(s >= 0), 'standard error must be non-negative'
p_zeta = mc.Uniform('p_zeta_%s'%name, 1.e-9, 10., value=1.e-6)
i_inf = pl.isinf(s)
@mc.observed(name='p_obs_%s'%name)
def p_obs(value=p, pi=pi, sigma=sigma, s=s, p_zeta=p_zeta):
return mc.normal_like(pl.log(value[~i_inf]+p_zeta), pl.log(pi[~i_inf]+p_zeta),
1./(sigma**2. + (s/(value+p_zeta))[~i_inf]**2.))
s_noninf = s.copy()
s_noninf[i_inf] = 0.
@mc.deterministic(name='p_pred_%s'%name)
def p_pred(pi=pi, sigma=sigma, s=s_noninf, p_zeta=p_zeta):
return pl.exp(mc.rnormal(pl.log(pi+p_zeta), 1./(sigma**2. + (s/(pi+p_zeta))**2.))) - p_zeta
return dict(p_zeta=p_zeta, p_obs=p_obs, p_pred=p_pred)
def log_normal(name, pi, sigma, p, s):
""" Generate PyMC objects for a lognormal model
:Parameters:
- `name` : str
- `pi` : pymc.Node, expected values of rates
- `sigma` : pymc.Node, dispersion parameters of rates
- `p` : array, observed values of rates
- `s` : array, standard error sizes of rates
:Results:
- Returns dict of PyMC objects, including 'p_obs' and 'p_pred' the observed stochastic likelihood and data predicted stochastic
"""
assert pl.all(p > 0), 'observed values must be positive'
assert pl.all(s >= 0), 'standard error must be non-negative'
i_inf = pl.isinf(s)
@mc.observed(name='p_obs_%s' % name)
def p_obs(value=p, pi=pi, sigma=sigma, s=s):
return mc.normal_like(pl.log(value), pl.log(pi + 1.e-9),
1. / (sigma**2. + (s / value)**2.))
s_noninf = s.copy()
s_noninf[i_inf] = 0.
@mc.deterministic(name='p_pred_%s' % name)
def p_pred(pi=pi, sigma=sigma, s=s_noninf):
return pl.exp(
mc.rnormal(pl.log(pi + 1.e-9),
1. / (sigma**2. + (s / (pi + 1.e-9))**2)))
return dict(p_obs=p_obs, p_pred=p_pred)
def normal_model(name, pi, sigma, p, s):
""" Generate PyMC objects for a normal model
:Parameters:
- `name` : str
- `pi` : pymc.Node, expected values of rates
- `sigma` : pymc.Node, dispersion parameters of rates
- `p` : array, observed values of rates
- `s` : array, standard error of rates
:Results:
- Returns dict of PyMC objects, including 'p_obs' and 'p_pred' the observed stochastic likelihood and data predicted stochastic
"""
assert pl.all(s >= 0), 'standard error must be non-negative'
i_inf = pl.isinf(s)
@mc.observed(name='p_obs_%s'%name)
def p_obs(value=p, pi=pi, sigma=sigma, s=s):
return mc.normal_like(value[~i_inf], pi[~i_inf], 1./(sigma**2. + s[~i_inf]**2.))
s_noninf = s.copy()
s_noninf[i_inf] = 0.
@mc.deterministic(name='p_pred_%s'%name)
def p_pred(pi=pi, sigma=sigma, s=s_noninf):
return mc.rnormal(pi, 1./(sigma**2. + s**2.))
return dict(p_obs=p_obs, p_pred=p_pred)
def likelihood(self, x0, X, Y, U):
"""returns the log likelihood of the states `X` and observations `Y`
under the current model p(X,Y|M)
Parameters
----------
x0 : matrix
initial state
X : list of matrix
state sequence
Y : list of matrix
observation sequence
U : list of matrix
input sequence
Notes
----------
This calculates
p(X,Y|M) = p(x0)\prod_{t=1}^Tp(y_t|x_t)\prod_{t=1}^Tp(x_t|x_{t-1})
using the model currently defined in self.
"""
l1 = pb.sum([pb.log(self.observation_dist(x,y)) for (x,y) in zip(X,Y)])
l2 = pb.sum([
pb.log(self.transition_dist(x,u,xdash)) for (x,u,xdash) in zip(X[:-1],U[:-1],X[1:])])
l3 = self.init_dist(x0)
l = l1 + l2 + l3
assert not pb.isinf(l).any(), (l1,l2,l3)
return l
def CreateFromAliFile(self):
self.LoadAligments(self.AliFile)
printStr = ''
self._lst_ignored_files = []
self.NumFrames = 0
created_means = False
for index, (file_name, utterance_id) in \
enumerate(zip(self.RawFileList, self.UtteranceIds)):
printStrNew = '\b' * (len(printStr)+1)
printStr = "Loading data for utterance #: " + str(index+1)
printString = printStrNew + printStr
print printString,
sys.stdout.flush()
data = HTK.ReadHTKWithDeltas(file_name)
if sum(isnan(data)) != 0 or sum(isinf(data)) != 0:
self._lst_ignored_files.append(index)
continue
if not created_means:
created_means = True
self.data_dim = data.shape[0]
self.__CreateMeansAndStdevs()
self.DataSumSq += (data**2).sum(axis=1).reshape(-1,1)
self.DataSum += data.sum(axis=1).reshape(-1,1)
self.NumFrames += data.shape[1]
if self.Utt2Speaker != None:
speaker = self.Utt2Speaker[utterance_id]
self.SpeakerMeans[speaker] += data.sum(axis=1).reshape(-1,1)
self.SpeakerStds[speaker] += (data**2).sum(axis=1).reshape(-1,1)
self.SpeakerNumFrames[speaker] += data.shape[1]
sys.stdout.write("\n")
for file_num in self._lst_ignored_files:
sys.stdout.write("File # " + str(file_num) + " was ignored \
because of errors\n")
if self.Utt2Speaker != None:
for speaker in self.Speaker2Utt.keys():
self.SpeakerMeans[speaker] /= (1.0 *self.SpeakerNumFrames[speaker])
self.SpeakerStds[speaker] -= self.SpeakerNumFrames[speaker] * \
(self.SpeakerMeans[speaker]**2)
self.SpeakerStds[speaker] /= (1.0 *self.SpeakerNumFrames[speaker]-1)
self.SpeakerStds[speaker][self.SpeakerStds[speaker] < 1e-8] = 1e-8
self.SpeakerStds[speaker] = sqrt(self.SpeakerStds[speaker])
self.DataMeanVect = self.DataSum/self.NumFrames
variances = (self.DataSumSq - self.NumFrames*(self.DataMeanVect**2))/(self.NumFrames-1)
variances[variances < 1e-8] = 1e-8
self.DataStdVect = sqrt(variances)
def spline(name, ages, knots, smoothing, interpolation_method='linear'):
""" Generate PyMC objects for a spline model of age-specific rate
Parameters
----------
name : str
knots : array
ages : array, points to interpolate to
smoothing : pymc.Node, smoothness parameter for smoothing spline
interpolation_method : str, optional, one of 'linear', 'nearest', 'zero', 'slinear', 'quadratic, 'cubic'
Results
-------
Returns dict of PyMC objects, including 'gamma' (log of rate at
knots) and 'mu_age' (age-specific rate interpolated at all age
points)
"""
assert pl.all(pl.diff(knots) > 0), 'Spline knots must be strictly increasing'
# TODO: consider changing this prior distribution to be something more familiar in linear space
gamma = [mc.Normal('gamma_%s_%d'%(name,k), 0., 10.**-2, value=-10.) for k in knots]
#gamma = [mc.Uniform('gamma_%s_%d'%(name,k), -20., 20., value=-10.) for k in knots]
# TODO: fix AdaptiveMetropolis so that this is not necessary
flat_gamma = mc.Lambda('flat_gamma_%s'%name, lambda gamma=gamma: pl.array([x for x in pl.flatten(gamma)]))
import scipy.interpolate
@mc.deterministic(name='mu_age_%s'%name)
def mu_age(gamma=flat_gamma, knots=knots, ages=ages):
mu = scipy.interpolate.interp1d(knots, pl.exp(gamma), kind=interpolation_method, bounds_error=False, fill_value=0.)
return mu(ages)
vars = dict(gamma=gamma, mu_age=mu_age, ages=ages, knots=knots)
if (smoothing > 0) and (not pl.isinf(smoothing)):
#print 'adding smoothing of', smoothing
@mc.potential(name='smooth_mu_%s'%name)
def smooth_gamma(gamma=flat_gamma, knots=knots, tau=smoothing**-2):
# the following is to include a "noise floor" so that level value
# zero prior does not exert undue influence on age pattern
# smoothing
# TODO: consider changing this to an offset log normal
gamma = gamma.clip(pl.log(pl.exp(gamma).mean()/10.), pl.inf) # only include smoothing on values within 10x of mean
return mc.normal_like(pl.sqrt(pl.sum(pl.diff(gamma)**2 / pl.diff(knots))), 0, tau)
vars['smooth_gamma'] = smooth_gamma
return vars
def spline(name, ages, knots, smoothing, interpolation_method='linear'):
""" Generate PyMC objects for a piecewise constant Gaussian process (PCGP) model
Parameters
----------
name : str
knots : array, locations of the discontinuities in the piecewise constant function
ages : array, points to interpolate to
smoothing : pymc.Node, smoothness parameter for smoothing spline
interpolation_method : str, optional, one of 'linear', 'nearest', 'zero', 'slinear', 'quadratic, 'cubic'
Results
-------
Returns dict of PyMC objects, including 'gamma' and 'mu_age'
the observed stochastic likelihood and data predicted stochastic
"""
assert pl.all(pl.diff(knots) > 0), 'Spline knots must be strictly increasing'
gamma = [mc.Normal('gamma_%s_%d'%(name,k), 0., 10.**-2, value=-10.) for k in knots]
#gamma = [mc.Uniform('gamma_%s_%d'%(name,k), -20., 20., value=-10.) for k in knots]
# TODO: fix AdaptiveMetropolis so that this is not necessary
flat_gamma = mc.Lambda('flat_gamma_%s'%name, lambda gamma=gamma: pl.array([x for x in pl.flatten(gamma)]))
import scipy.interpolate
@mc.deterministic(name='mu_age_%s'%name)
def mu_age(gamma=flat_gamma, knots=knots, ages=ages):
mu = scipy.interpolate.interp1d(knots, pl.exp(gamma), kind=interpolation_method, bounds_error=False, fill_value=0.)
return mu(ages)
vars = dict(gamma=gamma, mu_age=mu_age, ages=ages, knots=knots)
if (smoothing > 0) and (not pl.isinf(smoothing)):
print 'adding smoothing of', smoothing
@mc.potential(name='smooth_mu_%s'%name)
def smooth_gamma(gamma=flat_gamma, knots=knots, tau=smoothing**-2):
# the following is to include a "noise floor" so that level value
# zero prior does not exert undue influence on age pattern
# smoothing
gamma = gamma.clip(pl.log(pl.exp(gamma).mean()/10.), pl.inf) # only include smoothing on values within 10x of mean
return mc.normal_like(pl.sqrt(pl.sum(pl.diff(gamma)**2 / pl.diff(knots))), 0, tau)
vars['smooth_gamma'] = smooth_gamma
return vars
def run(self):
"""Runs in own thread - reads data via input counter task"""
self.measure_data = py.zeros((50, ))
try:
while self.running:
self.ctr.data = py.zeros((10000, ), dtype=py.uint32)
self.ctr.ReadCounterU32(10000, 10., self.ctr.data, 10000, None,
None)
self.ctr.StopTask()
self.measure_data[0] = (self.ctr.data[-1]) * 10
self.measure_data[py.isinf(self.measure_data)] = py.nan
self.measure_data = py.roll(self.measure_data, -1)
print self.measure_data
self.gotdata = True
finally:
self.running = False
self.ctr.StopTask()
self.ctr.ClearTask()
self.trig.ClearTask()
self.btn.setEnabled(True)
def spline(name, ages, knots, smoothing, interpolation_method='linear'):
""" Generate PyMC objects for a piecewise constant Gaussian process (PCGP) model
Parameters
----------
name : str
knots : array, locations of the discontinuities in the piecewise constant function
ages : array, points to interpolate to
smoothing : pymc.Node, smoothness parameter for smoothing spline
interpolation_method : str, optional, one of 'linear', 'nearest', 'zero', 'slinear', 'quadratic, 'cubic'
Results
-------
Returns dict of PyMC objects, including 'gamma' and 'mu_age'
the observed stochastic likelihood and data predicted stochastic
"""
assert pl.all(
pl.diff(knots) > 0), 'Spline knots must be strictly increasing'
gamma = [
mc.Normal('gamma_%s_%d' % (name, k), 0., 10.**-2, value=-10.)
for k in knots
]
#gamma = [mc.Uniform('gamma_%s_%d'%(name,k), -20., 20., value=-10.) for k in knots]
# TODO: fix AdaptiveMetropolis so that this is not necessary
flat_gamma = mc.Lambda(
'flat_gamma_%s' % name,
lambda gamma=gamma: pl.array([x for x in pl.flatten(gamma)]))
import scipy.interpolate
@mc.deterministic(name='mu_age_%s' % name)
def mu_age(gamma=flat_gamma, knots=knots, ages=ages):
mu = scipy.interpolate.interp1d(knots,
pl.exp(gamma),
kind=interpolation_method,
bounds_error=False,
fill_value=0.)
return mu(ages)
vars = dict(gamma=gamma, mu_age=mu_age, ages=ages, knots=knots)
if (smoothing > 0) and (not pl.isinf(smoothing)):
print 'adding smoothing of', smoothing
@mc.potential(name='smooth_mu_%s' % name)
def smooth_gamma(gamma=flat_gamma, knots=knots, tau=smoothing**-2):
# the following is to include a "noise floor" so that level value
# zero prior does not exert undue influence on age pattern
# smoothing
gamma = gamma.clip(
pl.log(pl.exp(gamma).mean() / 10.),
pl.inf) # only include smoothing on values within 10x of mean
return mc.normal_like(
pl.sqrt(pl.sum(pl.diff(gamma)**2 / pl.diff(knots))), 0, tau)
vars['smooth_gamma'] = smooth_gamma
return vars
def spline(name, ages, knots, smoothing, interpolation_method='linear'):
""" Generate PyMC objects for a spline model of age-specific rate
Parameters
----------
name : str
knots : array
ages : array, points to interpolate to
smoothing : pymc.Node, smoothness parameter for smoothing spline
interpolation_method : str, optional, one of 'linear', 'nearest', 'zero', 'slinear', 'quadratic, 'cubic'
Results
-------
Returns dict of PyMC objects, including 'gamma' (log of rate at
knots) and 'mu_age' (age-specific rate interpolated at all age
points)
"""
assert pl.all(
pl.diff(knots) > 0), 'Spline knots must be strictly increasing'
# TODO: consider changing this prior distribution to be something more familiar in linear space
gamma = [
mc.Normal('gamma_%s_%d' % (name, k), 0., 10.**-2, value=-10.)
for k in knots
]
#gamma = [mc.Uniform('gamma_%s_%d'%(name,k), -20., 20., value=-10.) for k in knots]
# TODO: fix AdaptiveMetropolis so that this is not necessary
flat_gamma = mc.Lambda(
'flat_gamma_%s' % name,
lambda gamma=gamma: pl.array([x for x in pl.flatten(gamma)]))
import scipy.interpolate
@mc.deterministic(name='mu_age_%s' % name)
def mu_age(gamma=flat_gamma, knots=knots, ages=ages):
mu = scipy.interpolate.interp1d(knots,
pl.exp(gamma),
kind=interpolation_method,
bounds_error=False,
fill_value=0.)
return mu(ages)
vars = dict(gamma=gamma, mu_age=mu_age, ages=ages, knots=knots)
if (smoothing > 0) and (not pl.isinf(smoothing)):
#print 'adding smoothing of', smoothing
@mc.potential(name='smooth_mu_%s' % name)
def smooth_gamma(gamma=flat_gamma, knots=knots, tau=smoothing**-2):
# the following is to include a "noise floor" so that level value
# zero prior does not exert undue influence on age pattern
# smoothing
# TODO: consider changing this to an offset log normal
gamma = gamma.clip(
pl.log(pl.exp(gamma).mean() / 10.),
pl.inf) # only include smoothing on values within 10x of mean
return mc.normal_like(
pl.sqrt(pl.sum(pl.diff(gamma)**2 / pl.diff(knots))), 0, tau)
vars['smooth_gamma'] = smooth_gamma
return vars
def sim(vx0,y0,L01,a0,k1,dE,m,ygrd,t,fs,steps):
# function for using fmin
# def optfunc_dE(par):
# return abs(-.5*k1*c**2+.5*(k1+par)*(c-(c*par/(k1+par)))**2-dE)
# function for using fsolve
def optfunc_dE(par, spring_par):
k1 = spring_par['k']
c = spring_par['c']
return abs(-.5*k1*c**2+.5*(k1+par)*(c-(c*par/(k1+par)))**2-dE)
g = -9.81 # gravitational force
betaTO_sim = zeros(steps)
xF=0
print 'x'
x_sim = [0]
y_sim = [0]
vx_sim = [0]
vy_sim = [0]
t_sim = [0]
k2 = 0
L02 = 0
skip_all = False
st_lo_idx = -1
# leg orientation
xL = L01 * cos(a0 * pi/180.)
yL = L01 * sin(a0 * pi/180.)
# first ground contact
t_gc = sqrt(2.*(L01*sin(a0*pi/180.)-y0)/g)
if isnan(t_gc) == True:
skip_all = True
if skip_all == False:
x_gc = vx0*t_gc
y_gc = .5*g*t_gc**2 + y0
# system parameter for first step
s = zeros(4)
s[0] = 0
s[1] = y0
s[2] = vx0
s[3] = 0
# definitions for calculation
end_sim = False
res = zeros([1,4])
res_app = []
t_v = [0]
skip_stance = False
# start loop
while end_sim == False:
st_lo_idx += 1
if st_lo_idx != 0: # if not first step
t_gc = (-res[-1,3]/g) + \
sqrt( ((res[-1,3]**2)/(g**2)) - \
((2*res[-1,1])/g-(2*L01*sin(a0*pi/180)/g)) )
if isnan(t_gc) == False and isinf(t_gc) == False:
y_gc = 0.5*g*t_gc**2 + res[-1,3]*t_gc + res[-1,1]
t_vec = linspace(0,t_gc,ceil(t_gc*fs)+1)
#print len(t_vec)
s[0] = res[-1,2]*t_gc + res[-1,0]
s[1] = y_gc
s[2] = res[-1,2]
s[3] = g*t_gc +res[-1,3]
else:
t_gc = 1
t_vec = linspace(0,t_gc,ceil(t_gc*fs)+1)
end_sim = True
skip_stance = True
#break
vx = zeros_like(t_vec)
vy = zeros_like(t_vec)
x = zeros_like(t_vec)
y = zeros_like(t_vec)
vx[0] = res[-1,2]
vy[0] = res[-1,3]
x[0] = res[-1,0]
y[0] = res[-1,1]
else: # if first step
t_vec = linspace(0,t_gc,ceil(t_gc*fs)+1)
#print t_vec
vx = zeros_like(t_vec)
vy = zeros_like(t_vec)
x = zeros_like(t_vec)
y = zeros_like(t_vec)
#print vx
vx[0] = s[2]
vy[0] = s[3]
x[0] = s[0]
y[0] = s[1]
### flight phase
x[1:] = vx[0]*t_vec[1:] + x[0]
y[1:] = .5*g*(t_vec[1:]**2)+vy[0]*t_vec[1:] + y[0]
vy[1:] = g*t_vec[1:] + vy[0]
vx[1:] = vx[0]*ones_like(t_vec[1:])
# end at apex
if st_lo_idx == steps or skip_stance == True:
tapex = vy[0]/-g
#print 'y0=',y0,'vy0=',vy[0],' tapex=',tapex
tvec_apex = linspace(0,tapex,ceil(tapex*fs)+1)
vyapex = g*tvec_apex+vy[0]
#print 'vyapex=',vyapex
yapex = .5*g*(tvec_apex**2) + vyapex[0]*tvec_apex + y[0]
xapex = vx[0] * tvec_apex + x[0]
vxapex = vx[0] * ones_like(tvec_apex)
#print 'lvy:',len(vyapex),' lvx:',len(vxapex)
x_sim = append(x_sim,xapex[1:])
y_sim = append(y_sim,yapex[1:])
vx_sim = append(vx_sim,vxapex[1:])
vy_sim = append(vy_sim,vyapex[1:])
t_sim = append(t_sim,tvec_apex[1:]+t_sim[-1])
skip_stance = True
"""
for kk in arange(2,len(y)):
if y[kk-2]<y[kk-1] and y[kk-1]>y[kk]:
end_sim = True
x_sim = append(x_sim,x[1:kk])
y_sim = append(y_sim,y[1:kk])
vx_sim = append(vx_sim,vx[1:kk])
vy_sim = append(vy_sim,vy[1:kk])
t_sim = append(t_sim,t_vec[1:kk]+t_sim[-1])
"""
### stance phase
if skip_stance == False:
# system parameter
if st_lo_idx == 0: # if first step
s[0] = x_gc # x
s[1] = y_gc # y
s[2] = vx0 # vx
s[3] = g*t_gc + 0 # vy
xF = s[0]+xL
yF = ygrd
parameter = {'k':k1,'L0':L01,'m':m,'g':g,'xF':xF,'yF':yF}
calc_length = 0.1*fs
find_pos = False
round_prec = 11 # precision of rounding
#print 'round_prec = ', round_prec
step_size = 1./fs
res = zeros([1,4])
t_v = [0]
res_app = False
skip = False
L0 = L01
FinMin = False
# start integration of stance phase
while find_pos == False:
t_vint = (step_size) * arange(calc_length)
res_int = odeint(s_dot,s,t_vint, args=(parameter,), rtol=1e-11)
# stop condition vx or y < 0
if res_int[-1,1] < 0 or res_int[-1,2] < 0:
find_pos = True
end_sim = True
skip = True
# if ymin: calculation of new k and L for energy changes
for idx in arange(1,len(res_int[:,1])-1):
if res_int[idx,1]<res_int[idx-1,1] and res_int[idx,1]<res_int[idx+1,1]:
res_int=delete(res_int,s_[idx+1::],0)
t_vint = delete(t_vint,s_[idx+1::],0)
c= L0-(sqrt(res_int[-1,1]**2 + (xF-res_int[-1,0])**2))
#dk = fmin(optfunc_dE,0,full_output=1,disp=0,xtol=1e-12)
spring_par = {'k':k1,'c':c}
dk = fsolve(optfunc_dE,x0=[0],args=(spring_par,))
#print '###### dk ####',dk
# if not find a minimum stop loop --> no step
#if dk[-1] <> 0:
# find_pos = True
# end_sim = True
# skip = True
# raise ValueError, 'no energy management possible'
# break
k2 = k1+dk[0]
L02 = L01 - c*dk[0]/(k1+dk[0])
#print 'delta L = ',L0 - L02, ' delta k = ', dk[0]
L0 = L02
parameter = {'k':k2,'L0':L02,'m':m,'g':g,'xF':xF,'yF':yF}
FinMin = True
break
# if not ymin: start one before to avoid ignoring while change
if FinMin == False:
res_int = res_int[:-1]
t_vint = t_vint[:-1]
# virtual leg after integration
L_virt = sqrt(res_int[-1,1]**2 + (xF-res_int[-1,0])**2)
if L_virt < L0 and skip == False:
s[0] = res_int[-1,0]
s[1] = res_int[-1,1]
s[2] = res_int[-1,2]
s[3] = res_int[-1,3]
elif L_virt >= L0 and skip == False:
for jj in arange(1,len(res_int)):
L_virt_1 = sqrt(res_int[jj,1]**2 + abs(xF-res_int[jj,0])**2)
L_virt_2 = sqrt(res_int[jj-1,1]**2 + abs(xF-res_int[jj-1,0])**2)
if round(L_virt_2,round_prec) == round(L0,round_prec):
find_pos = True
res_int = res_int[0:jj,:]
res_app = True
t_vint = t_vint[0:jj]
if res_int[-1,3] <=0: end_sim = True
break
elif L_virt_2 < L0 and L_virt_1 > L0:
s[0] = res_int[jj-1,0]
s[1] = res_int[jj-1,1]
s[2] = res_int[jj-1,2]
s[3] = res_int[jj-1,3]
res_int = res_int[0:jj,:]
res_app = True
t_vint = t_vint[0:jj]
step_size = step_size / 10.
break
# append arrays
if res_app == False:
res = res_int
t_v = t_vint
res_app = True
else:
res = append(res,res_int[1::],0)
t_v = append(t_v,t_vint[1::]+t_v[-1])
# take of angle
#print st_lo_idx
betaTO_sim[st_lo_idx] = arctan(res[-1,1]/(res[-1,0]-xF))*180./pi
# append arrays
x_sim = append(x_sim,append(x,res[:,0]))
y_sim = append(y_sim,append(y,res[:,1]))
vx_sim = append(vx_sim,append(vx,res[:,2]))
vy_sim = append(vy_sim,append(vy,res[:,3]))
t_sim = append(t_sim,append(t_vec,t_v+t_gc)+t_sim[-1])
else: #skip_stance == True because of ending at apex
end_sim = True
# cut array
x_sim = x_sim[1::]
y_sim = y_sim[1::]
vx_sim = vx_sim[1::]
vy_sim = vy_sim[1::]
t_sim = t_sim[1::]
#print 'vor return'
return x_sim,y_sim,vx_sim,vy_sim,t_sim,st_lo_idx,betaTO_sim,xF,k2,L02
