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)