In [1]:
# BSLIP test
#define BSLIP elements
import time
from libshai import integro
from copy import deepcopy
#state: x,y,vx, vy
def rhs_ss(t, y, pars):
k, l0, alpha, m, g = pars['k'], pars['l0'], pars['alpha'], pars['m'], pars['g']
foot = pars['foot1']
leg = y[:2] - foot
leglength = norm(leg)
F = max(k * (l0 - leglength), 0)
Fx = F * leg[0] / leglength
Fy = F * leg[1] / leglength
return array([y[2], y[3], Fx/m, Fy/m + g])
def rhs_ds(t, y, pars):
k, l0, alpha, m, g = pars['k'], pars['l0'], pars['alpha'], pars['m'], pars['g']
foot = pars['foot1']
leg = y[:2] - foot
leglength = norm(leg)
foot2 = pars['foot2']
leg2 = y[:2] - foot2
leglength2 = norm(leg2)
F = max(k * (l0 - leglength), 0)
Fx = F * leg[0] / leglength
Fy = F * leg[1] / leglength
F2 = max(k * (l0 - leglength2), 0)
Fx2 = F2 * leg2[0] / leglength2
Fy2 = F2 * leg2[1] / leglength2
return array([y[2], y[3], (Fx + Fx2)/m, (Fy + Fy2)/m + g])
def takeoff_event(t, states, traj, p):
# take-off: when leg length of leg 2 reaches l0
leglength = norm(states[1][:2] - p['foot2'])
if leglength > p['l0']:
# check if previous was < l0
leglength = norm(states[0][:2] - p['foot2'])
if leglength <= p['l0']:
return True
return False
def takeoff_event_refine(t, state, pars): #required signature: t,
leglength = norm(state[:2] - pars['foot2'])
#print ">", leglength
return - (leglength - pars['l0'])
def touchdown_event(t, states, traj, p):
# take-off: when leg length of leg 2 reaches l0
if states[1][1] - p['l0'] * sin(p['alpha']) <= 0:
if states[0][1] - p['l0'] * sin(p['alpha']) > 0:
return True
return False
def touchdown_event_refine(t, state, func_params): #required signature: t,
return - (state[1] - func_params['l0'] * sin(func_params['alpha']))
# "Midstance" event: Vertical Leg Orientation
def vlo_event(t, states, traj, p):
# vlo: CoM above foot
if states[1][0] - p['foot1'][0] >= 0:
return True
return False
def vlo_event_refine(t, state, pars): #required signature: t,
return -(state[0] - pars['foot1'][0])
In [4]:
p0 = {'k' : 20000.,
'alpha' : 76. * pi / 180.,
'l0' : 1.,
'm' : 80.,
'g' : -9.81,
'foot1' : array([0, 0]),
'foot2' : array([-1, 0]),
}
IC = array([0, .98, 1.12, 0])
#IC =
def adj_speed(IC, p):
E_des = 816. # J
E_k = .5 * p['k'] * (IC[1] - p['l0'])**2
E_pot = - p['m'] * p['g'] * IC[1]
E_kin = .5 * p['m'] * norm(IC[2:])**2
print "total energy:", E_k + E_pot + E_kin
vx = sqrt( 2 * (E_des - E_k - E_pot) / p['m'] - IC[-1]**2)
# adjust horizontal velocity
res = array(IC)
res[2] = vx
return res
tE = 0.
ICx = array(IC)
all_res = []
all_time = []
In [10]:
t0 = time.time()
px = deepcopy(p0)
IC = array([0, .9810132, 1.12, 0])
IC = array([0, .97823, 1.12, 0])
IC = array([0, .9794348, 1.12, 0])
# from VLO until touchdown
oss2 = integro.odeDP5(rhs_ss, pars=px)
oss2.event = touchdown_event
oss2.ODE_ATOL = 1e-12
oss2.ODE_RTOL = 1e-12
oss2.ODE_EVTTOL = 1e-12
# from touchdown to takeoff
ods = integro.odeDP5(rhs_ds, pars=px)
ods.ODE_ATOL = 1e-12
ods.ODE_RTOL = 1e-12
ods.ODE_EVTTOL = 1e-12
ods.event = takeoff_event
# from takeoff until VLO
oss1 = integro.odeDP5(rhs_ss, pars=px)
oss1.event = vlo_event
oss1.ODE_ATOL = 1e-12
oss1.ODE_RTOL = 1e-12
oss1.ODE_EVTTOL = 1e-12
tE = 0.
all_res = []
all_time = []
ICx = adj_speed(IC, px)
print ICx
print "v" * 5
tvec0 = linspace(tE, tE + 1, 200)
ts, ys = oss2(ICx, tvec0, dt=1e-2)
oss2.refine(lambda t, y: touchdown_event_refine(t, y, px) )
tE = ts[-1]
ICx = ys[-1, :]
all_res.append(ys.copy())
all_time.append(ts.copy())
# update feet positions
px['foot2'] = px['foot1'].copy()
px['foot1'] = array([ys[-1, 0] + cos(px['alpha']) * px['l0'], 0])
print "-" * 5
# double stance phase
tvec0 = linspace(tE, tE + 1, 200)
td, yd = ods(ICx, tvec0, dt=1e-2)
print "takeoff:", yd[-1, :], yd[-2,:]
ods.refine(lambda t, y: takeoff_event_refine(t, y, px) )
tE = td[-1]
ICx = yd[-1, :]
all_res.append(yd.copy())
all_time.append(td.copy())
print "=" * 5
# single stance until vlo
tvec0 = linspace(tE, tE + 1, 200)
ts2, ys2 = oss1(ICx, tvec0, dt=1e-2)
oss1.refine(lambda t, y: vlo_event_refine(t, y, px) )
tE = ts2[-1]
ICx = ys2[-1, :]
all_res.append(ys2.copy())
all_time.append(ts2.copy())
print "elapsed time:", time.time() - t0
#sres = vstack(all_res)
#stime = hstack(all_time)
figure()
plot(all_res[0][:,0], all_res[0][:,1],'bd')
plot(all_res[1][:,0], all_res[1][:,1],'r.')
plot(all_res[2][:,0], all_res[2][:,1],'y.')
#print px
print "Difference:", ys2[-1,1] - IC[1]
In [4]:
IC[1] , ys2[-1,1]
def getE(x,p):
E_k = .5 * p['k'] * (x[1] - p['l0'])**2
E_pot = - p['m'] * p['g'] * x[1]
E_kin = .5 * p['m'] * norm(x[2:])**2
print "E: ", E_k + E_pot + E_kin
return E_k + E_pot + E_kin
getE(ICx, px)
getE(ys2[-1,:], px)
pass
In [5]:
print ys2[-1,1], IC[1]
print ys2[-1,0], px['foot1'][0]
In [7]:
px
Out[7]:
In [20]:
Out[20]:
In [37]:
def refiner(sres, t_vec, p, fun):
sgn0 = fun(t_vec[0], sres[0,:], p)
for row in range(1, sres.shape[0]):
if fun(t_vec[row], sres[row,:], p) * sgn0 <= 0:
break
c = -fun(t_vec[row-1], sres[row-1,:], p) / (fun(t_vec[row], sres[row,:], p) - fun(t_vec[row-1], sres[row-1,:], p))
#print "c=", c
if abs(c) > 1:
raise ValueError("no event detected!")
res_final = sres[row-1] + c*(sres[row] - sres[row-1])
t_final = t_vec[row-1] + c*(t_vec[row] - t_vec[row-1])
res = sres[:row,:]
tvec = t_vec[:row]
tvec[-1] = t_final
res[-1,:] = res_final
return res, t_vec
In [42]:
px = deepcopy(p0)
IC = array([0, .9920132, 1.12, 0])
IC = array([0, .97899, 1.12, 0])
#IC = array([0, .97823, 1.12, 0])
import scipy.integrate as si
t0 = time.time()
tE = 0.
all_res = []
all_time = []
ICx = adj_speed(IC, px)
ICx = adj_speed(ICx, px)
print ICx
for rep in range(20):
#single stance until touchdown
tvec0 = linspace(tE, tE + 1, 200)
rhs_ss2 = lambda y, t, p: rhs_ss(t, y, p) # adapt call signature
res = si.odeint(rhs_ss2, y0=ICx, t=tvec0, args=(px,), rtol=1e-12, atol=1e-10)
# refine res to touchdown_event
res,tvec0 = refiner(res, tvec0, px, touchdown_event_refine)
all_res.append(res.copy())
all_time.append(tvec0.copy())
# update feet positions
px['foot2'] = px['foot1'].copy()
px['foot1'] = array([res[-1, 0] + cos(px['alpha']) * px['l0'], 0])
#double stance
tE = tvec0[-1]
tvec0 = linspace(tE, tE + 1, 200)
rhs_ds2 = lambda y, t, p: rhs_ds(t, y, p) # adapt call signature
res = si.odeint(rhs_ds2, y0=res[-1,:], t=tvec0, args=(px,), rtol=1e-12, atol=1e-10, hmax=1e-3)
# refine res to touchdown_event
res, tvec0 = refiner(res, tvec0, px, takeoff_event_refine)
all_res.append(res.copy())
all_time.append(tvec0.copy())
#single stance until VLO
tE = tvec0[-1]
tvec0 = linspace(tE, tE + 1, 200)
res = si.odeint(rhs_ss2, y0=res[-1,:], t=tvec0, args=(px,), rtol=1e-12, atol=1e-10, hmax=1e-3)
# refine res to touchdown_event
res,tvec0 = refiner(res, tvec0, px, vlo_event_refine)
all_res.append(res.copy())
all_time.append(tvec0.copy())
tE = tvec0[-1]
ICx = res[-1,:]
print "elapsed time:", time.time() - t0
#sres = vstack(all_res)
#stime = hstack(all_time)
figure()
sim_res = vstack(all_res)
plot(sim_res[:,0], sim_res[:,1],'b.')
#plot(all_res[0][:,0], all_res[0][:,1],'bd')
#plot(all_res[1][:,0], all_res[1][:,1],'r+')
#plot(all_res[2][:,0], all_res[2][:,1],'y.')
#print px
print "Difference:", res[-1,1] - ICx[1]
In [22]:
print all_res[0][-1,1]
print sin(76. * pi / 180)'
In [ ]:
In [ ]:
res,tvec0 = refiner(res, tvec0, px, touchdown_event_refine)
In [ ]:
t
In [ ]:
res.shape
In [ ]:
res[4][1] - sin(76.*pi/180)
In [ ]:
row = 1
touchdown_event_refine(t, r, px)
In [ ]:
res.resize((10,4))
In [ ]:
resize(