# set noise paramters

# time delay range [1,5] time steps

# divegence sensor noise [0,0.15]s-1

# thrust time constant [0.02, 0.1]s

dyn = quad_landing(delay=ceil(4*np.random.random_sample())+1,

noise=0.1*np.random.random_sample()+0.05,

noise_p=0.25*np.random.random_sample(),

thrust_tc=0.04*np.random.random_sample()+0.005,

dt=1./ceil(20*np.random.random_sample() + 30),

computational_delay_prob=np.random.random_sample()/5.

)

h0 = [2., 4., 6., 8.]

t_score = 0.

h_score = 0.

v_score = 0.

for i in range(len(h0)):

obs = dyn.reset()

individual[0].reset()

dyn.set_h0(h0[i])

done = False

#energy = 0.

while not done:

obs, _, done, _ = dyn.step(individual[0].predict(obs, dyn.t))

#energy += dyn.thrust_cmd + dyn.G

t = dyn.t

h = dyn.y[0]

v = dyn.y[1]

# penalize not landing, here only the hieght matters

if t >= dyn.MAX_T or h >= dyn.MAX_H:

v = -2.

t = dyn.MAX_T

# don't differentiate hieght score between sucessful individuals

#if h <= dyn.MIN_H:

# h = dyn.MIN_H

# don't differentiate velocity score between sucessful individuals

#if np.abs(v) <= 0.01:

# v = 0.

# penalize high speed crashing, not a viable solution

#if v < -2.:

# h = dyn.MAX_H

# t = dyn.MAX_T

t_score += t

h_score += h

v_score += v*v #np.abs(v)

# minimize time to end of sim, final height and velocity

random.seed(seed)

NGEN = 250

MU = 100

log_interval = 25

stats = tools.Statistics(lambda ind: ind.fitness.values)

stats.register("avg", np.mean, axis=0)

stats.register("std", np.std, axis=0)

stats.register("min", np.min, axis=0)

stats.register("max", np.max, axis=0)

stats.register("median", np.median, axis=0)

logbook = tools.Logbook()

logbook.header = "gen", "evals", "std", "min", "avg", "max", "median"

pop = toolbox.population(n=MU)

hof = tools.ParetoFront()

# Evaluate the individuals with an invalid fitness

invalid_ind = [ind for ind in pop if not ind.fitness.valid]

fitnesses = toolbox.map(toolbox.evaluate, invalid_ind)

for ind, fit in zip(invalid_ind, fitnesses):

ind.fitness.values = fit

# This is just to assign the crowding distance to the individuals

# no actual selection is done

pop = toolbox.select(pop, len(pop))

record = stats.compile(pop)

logbook.record(gen=0, evals=len(invalid_ind), **record)

print(logbook.stream)

hof.update(pop)

basepath = os.path.dirname(os.path.abspath(__file__))

log_dir = '{}/logs/{}/'.format(basepath, time.strftime('%y%m%d-%H%M%S'))

if not os.path.exists(log_dir):

os.makedirs(log_dir)

# log initial population

os.makedirs('{}0'.format(log_dir))

for i, agent in enumerate(pop):

agent[0].save_weights('{}0/{}_weights.csv'.format(log_dir, i), overwrite=True)

log_population(pop, '{}0'.format(log_dir))

# Begin the generational process

for gen in range(1, NGEN+1):

# Get Offspring

# first get pareto front

pareto_fronts = tools.sortNondominated(pop, len(pop))

selection = pareto_fronts[0]

len_pareto = len(pareto_fronts[0])

rest = list(chain(*pareto_fronts[1:]))

if len(rest) % 4:

rest.extend(random.sample(selection, 4 - (len(rest) % 4)))

selection.extend(tools.selTournamentDCD(rest, len(rest)))

offspring = [toolbox.mutate(toolbox.clone(ind)) for ind in selection[:len(pop)]]

# Revaluate the individuals in last population

fitnesses = toolbox.map(toolbox.evaluate, pop)

for ind, fit in zip(pop, fitnesses):

ind.fitness.values = fit

# Evaluate the new offspring

fitnesses = toolbox.map(toolbox.evaluate, offspring)

for ind, fit in zip(offspring, fitnesses):

ind.fitness.values = fit

# Update the hall of fame with the generated individuals

hof.update(offspring)

plot_population(pop, offspring, lim = [[10,120],[0,0],[0,4]])

# Select the next generation population

pop = toolbox.select(pop + offspring, MU)

pareto_fronts = tools.sortNondominated(pop, len(pop))

plot_selection(pop, pareto_front_size=len(pareto_fronts[0]), lim = [[10,120],[0,0],[0,4]])

record = stats.compile(pop)

logbook.record(gen=gen, evals=len(offspring)+len(pop), **record)

print(logbook.stream)

if gen % log_interval == 0 or gen == NGEN:

os.makedirs('{}{}'.format(log_dir, gen))

for i, agent in enumerate(pop):

agent[0].save_weights('{}{}/{}_weights.csv'.format(log_dir, gen, i), overwrite=True)

log_population(pop, '{}{}'.format(log_dir, gen))

with open('{}/gen_stats.txt'.format(log_dir), 'w') as fp:

np.savetxt(fp, logbook, fmt="%s")

plot_population(pop)

print("Final population hypervolume is %f" % hypervolume(pop, [11.0, 11.0, 11.0]))

os.makedirs('{}hof'.format(log_dir))

for i, agent in enumerate(hof):

agent[0].save_weights('{}hof/{}_weights.csv'.format(log_dir, i), overwrite=True)

log_population(hof, '{}hof'.format(log_dir))