class Dataset:
def __init__(self, datafile=dataRootPath + '/grid.npy'):
self.grid = GridPolicy()
self.grid.load(datafile)
for d in self.grid.data: # Correct error at the end of U traj
d.U[-1, :] = d.U[-2, :]
# For quad copter: remove terminal traj
self.data = [
d for d in self.grid.data if len(d.X) > 0 and abs(d.x0[2, 0]) < .7
]
def set(self):
data = self.data
SHIFT = 4
nex = lambda X: np.vstack([X[SHIFT:, :]] + [X[-1:, :]] * SHIFT
) # shit 5 times
xs = [d.X for d in data]
self.xs = np.array(np.vstack(xs))
us = [d.U for d in data]
self.us = np.array(np.vstack(us))
nexs = [nex(d.X) for d in data]
self.nexs = np.array(np.vstack(nexs))
vs = [d.cost - d.T for d in data]
self.vs = np.expand_dims(np.array(np.concatenate(vs)), 1)
subtraj = lambda X, i: np.ravel(np.vstack([X[i:, :]] + [X[-1:, :]] * i)
)
xplust = lambda X, T: np.hstack([X, np.expand_dims(T[-1] - T, 1)])
trajxs = [subtraj(d.X, i) for d in data for i in range(len(d.X))]
self.trajxs = np.array(np.vstack(trajxs))
trajus = [subtraj(d.U, i) for d in data for i in range(len(d.U))]
self.trajus = np.array(np.vstack(trajus))
return self
np .random.seed (RANDOM_SEED)
random.seed (RANDOM_SEED)
#dataRootPath = dataRootPath + '/2dgrid'
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,