def simulate(qm, nb=10000):
print 'Simulation'
for i in xrange(nb):
if (i % 1000) == 0:
print i
xt=rd.uniform(XMIN,XMAX)
dxt=rd.uniform(VMIN,VMAX)
ut=rd.uniform(UMIN,UMAX)
res=lip.simulate([xt,dxt],[ut])
# xt1=res[0]+random.randn()*0.01 #+rd.gauss(0,0.01)
# dxt1=res[1]+random.randn()*0.01#+rd.gauss(0,0.01)
xt1=rd.gauss(res[0], 0.01)
dxt1=rd.gauss(res[1], 0.01)
xpos=Xpos.Discretize((xt))
xvel=Xvel.Discretize((dxt))
u=U.Discretize((ut))
xposres=Xpos.Discretize((xt1))
xvelres=Xvel.Discretize((dxt1))
# print 'SIMU ',(xpos,xvel),(xposres,xvelres),(u)
# print xt,dxt,ut,res[0],res[1],xt1,dxt1
qm.OnlineUpdate((xpos,xvel),(xposres,xvelres),(u, ),1.0)
def ComputeStep(rx):
xi,vi,ui=XposXvel_XposXvelU.RDistrib.GetIdxFromFlat(rx)
# ui,vi,xi=XposXvel_XposXvelU.RDistrib.GetIdxFromFlat(rx)
# print rx, xi, vi, ui
x=Xpos.Continuize(xi)
v=Xvel.Continuize(vi)
u=U.Continuize(ui)
Xnext=lip.simulate([x,v],[u])
#put some Gaussian value around
pxpos=0.0
pxvel=0.0
#dirty hack to keep track of the relevent non zero elements indexes
nzpos_idx={}
nzvel_idx={}
res={}
for li in xrange(Xpos_XposXvelU.GetNumLElements()): #iterate on Xpos only as we assume independance
lidx=Xpos_XposXvelU.LDistrib.GetIdxFromFlat(li)
# pxpos=gaussian(Xpos.Continuize(lidx),Xnext[0],SIGMA_POS)
# pxvel=gaussian(Xvel.Continuize(lidx),Xnext[1],SIGMA_VEL)
# print Xpos.GetDist(Xpos.Continuize(lidx),Xnext[0])[0], Xvel.GetDist(Xvel.Continuize(lidx),Xnext[1])[0]
pxpos=gaussian_dist(Xpos.GetDist(Xpos.Continuize(lidx),Xnext[0])[0],SIGMA_POS)
pxvel=gaussian_dist(Xvel.GetDist(Xvel.Continuize(lidx),Xnext[1])[0],SIGMA_VEL)
# condidx=[xi,vi,ui,]+lidx
# condidx=lidx+(xi,vi,ui)
if pxpos > _P_THRES:
nzpos_idx[lidx[0]]=pxpos #in a general case it should be a tuple
if pxvel > _P_THRES:
nzvel_idx[lidx[0]]=pxvel
for posidx in nzpos_idx:
for velidx in nzvel_idx:
# print posidx,velidx
# XposXvel_XposXvelU.SetProbLR((posidx,velidx),(xi,vi,ui),nzpos_idx[posidx]*nzvel_idx[velidx])
# return (posidx,velidx),(xi,vi,ui),nzpos_idx[posidx]*nzvel_idx[velidx]
res[(posidx,velidx)+(xi,vi,ui)]=nzpos_idx[posidx]*nzvel_idx[velidx]
return res
def generate(XRANGE, DXRANGE, URANGE):
xt = rd.uniform(XRANGE[0], XRANGE[1])
dxt = rd.uniform(DXRANGE[0], DXRANGE[1])
ut = rd.uniform(URANGE[0], URANGE[1])
res = lip.simulate([xt, dxt], [ut])
xt1 = res[0]
dxt1 = res[1]
return [xt, dxt, ut, xt1, dxt1]
def generate(XRANGE, DXRANGE, URANGE):
xt = rd.uniform(XRANGE[0], XRANGE[1])
dxt = rd.uniform(DXRANGE[0], DXRANGE[1])
ut = rd.uniform(URANGE[0], URANGE[1])
res = lip.simulate([xt, dxt], [ut])
xt1 = res[0]
dxt1 = res[1]
return [xt, dxt, ut, xt1, dxt1]
def ComputeStepAll(rx):
xi,vi,ui=XposXvel_XposXvelU.RDistrib.GetIdxFromFlat(rx)
# ui,vi,xi=XposXvel_XposXvelU.RDistrib.GetIdxFromFlat(rx)
x=Xpos.Continuize(xi)
v=Xvel.Continuize(vi)
u=U.Continuize(ui)
Xnext=lip.simulate([x,v],[u])
#put some Gaussian value around
pxpos=0.0
pxvel=0.0
#dirty hack to keep track of the relevent non zero elements indexes
nzpos_idx={}
nzvel_idx={}
res={}
for li in xrange(Xpos_XposXvelU.GetNumLElements()):
for ri in xrange(Xvel_XposXvelU.GetNumLElements()):
lidx=Xpos_XposXvelU.LDistrib.GetIdxFromFlat(li)
ridx=Xvel_XposXvelU.LDistrib.GetIdxFromFlat(li)
# pxpos=gaussian(Xpos.Continuize(lidx),Xnext[0],SIGMA_POS)
# pxvel=gaussian(Xvel.Continuize(lidx),Xnext[1],SIGMA_VEL)
# print Xpos.GetDist(Xpos.Continuize(lidx),Xnext[0])[0], Xvel.GetDist(Xvel.Continuize(lidx),Xnext[1])[0]
pxpos=gaussian_dist(Xpos.GetDist(Xpos.Continuize(lidx),Xnext[0])[0],SIGMA_POS)
pxvel=gaussian_dist(Xvel.GetDist(Xvel.Continuize(ridx),Xnext[1])[0],SIGMA_VEL)
res[(posidx,velidx)+(xi,vi,ui)]=pxpos * pxvel
return res
def apply_torque(self, u):
self.x = simple_lip.simulate(self.x, [u], self.dt)
def apply_torque(self, u):
self.x = simple_lip.simulate(self.x, [u], self.dt)
def simulate_all(qm,nb=100):
print 'Simulation'
for i in xrange(nb):
print i
for xi in xrange(XCARD):
for vi in xrange(VCARD):
for ui in xrange(UCARD):
xt = Xpos.Continuize(xi)
dxt = Xvel.Continuize(vi)
ut = U.Continuize(ui)
res=lip.simulate([xt,dxt],[ut])
# xt1=res[0]+random.randn()*0.01 #+rd.gauss(0,0.01)
# dxt1=res[1]+random.randn()*0.01#+rd.gauss(0,0.01)
# xt1=res[0]+rd.gauss(0,0.01)
# dxt1=res[1]+rd.gauss(0,0.01)
# xt1=res[0]
# dxt1=res[1]
xt1=rd.gauss(res[0], SIGMA_POS)
dxt1=rd.gauss(res[1], SIGMA_VEL)
# n1 = rd.gauss(0, SIGMA_POS)
# n2 = rd.gauss(0, SIGMA_VEL)
# xt1=res[0] + n1
# dxt1=res[1] + n2
# xt1=rd.gauss(res[0], 0.01)
# dxt1=rd.gauss(res[1], 0.01)
# xt1 = res[0]
# dxt1 = res[1]
# print xi, vi, xt, dxt,res, xt1, dxt1
# xpos=Xpos.Discretize((xt))
# xvel=Xvel.Discretize((dxt))
# u=U.Discretize((ut))
xpos = xi
xvel = vi
u = ui
xposres=Xpos.Discretize((xt1))
xvelres=Xvel.Discretize((dxt1))
# xposres=Xpos.Discretize((res[0] + n1))
# xvelres=Xvel.Discretize((res[1] + n2))
# print 'SIMU ',(xpos,xvel),(xposres,xvelres),(u)
# print xt,dxt,ut,res[0],res[1],xt1,dxt1
qm.OnlineUpdate((xpos,xvel),(xposres,xvelres),(u, ),1.0)
