grid = GridPolicy(prm)
EPS = 1e-3
grid.setGrid( np.concatenate([ env.qlow, zero(3) ]),
np.concatenate([ env.qup , zero(3)+EPS ]), .1 )
#grid.setGrid( np.matrix([ -1., -1., 0, 0, 0, 0 ]).T,
# np.matrix([ 1., 1., 0, 0, 0, 0 ]).T+EPS, .1 )
config(acado,'policy')
acado.setup_async(32,200)
if LOAD_GRID:
grid.load(dataRootPath+'/grid.npy')
if SAMPLE_GRID:
print 'Sample the grid',time.ctime()
grid.sample(subsample=1,verbose=True)
np.save(dataRootPath+'/grid_sampled.npy',grid.data)
print 'Sampling done',time.ctime()
if len(REFINE_GRID)>0:
config(acado,'refine')
if 1 in REFINE_GRID:
print 'Fill the grid',time.ctime()
grid.refineGrid(NNEIGHBOR=30,PERCENTAGE=.9,
RANDQUEUE=[ i for i,d in enumerate(grid.data) if d.cost>100])
grid.refineGrid(NNEIGHBOR=100,PERCENTAGE=.9,
RANDQUEUE=[ i for i,d in enumerate(grid.data) if d.cost>100])
np.save(dataRootPath+'/grid_filled.npy',grid.data)
if 2 in REFINE_GRID:
connexifyPrm(graph)
prm.graph.save(dataRootPath)
# --- GRID ---
# --- GRID ---
# --- GRID ---
oprm = OptimalPRM.makeFromPRM(prm,acado=prm.connect.acado,stateDiff=PendulumStateDiff(2))
grid = GridPolicy(oprm)
EPS = 1e-3
grid.setGrid([ -np.pi,-np.pi+EPS,0,0],[np.pi,np.pi-EPS,EPS,EPS],1.)
if SAMPLE_GRID:
print 'Sample the grid',time.ctime()
grid.sample()
else:
grid.load(dataRootPath+'/grid.npy')
if REFINE_GRID>3:
print 'Fill the grid',time.ctime()
refineGrid(data,NNEIGHBOR=30,PERCENTAGE=.9,
RANDQUEUE=[ i for i,d in enumerate(data) if d.cost>100])
refineGrid(data,NNEIGHBOR=100,PERCENTAGE=.9,
RANDQUEUE=[ i for i,d in enumerate(data) if d.cost>100])
np.save(dataRootPath+'/grid_filled.npy',data)
if REFINE_GRID>3:
print 'Refine the grid',time.ctime()
refineGrid(data,5000,NNEIGHBOR=20,PERCENTAGE=.9)
np.save(dataRootPath+'/grid.npy',data)