class GeneticAlgorithm():
def __init__(self,
world=None,
NGEN=1000,
n_ind=100,
n_elite=10,
fitness_thresh=0.1,
margin_on=False,
verbose=True):
self.trajectory = [world.start]
self.history = []
self.pop = []
self.best_ind = []
self.gtot = 0
self.n_ind = n_ind # The number of individuals in a population.
self.n_elite = n_elite
self.NGEN = NGEN # The number of generation loop.
self.fitness_thresh = fitness_thresh
self.verbose = verbose
# --- Generate Collision Checker
self.world = world
if margin_on:
self.world_margin = world.mcopy(rate=1.05)
else:
self.world_margin = world
self.cc = CollisionChecker(world)
self.ccm = CollisionChecker(self.world_margin)
def create_pop_prm(self, m, n):
prm_list = [PRM(self.world_margin, 30, 3) for i in range(m)]
path_list = []
for prm in prm_list:
prm.single_query()
if prm.path_list != []:
prm.multi_query(n)
path_list += prm.path_list
return [Individual(path) for path in path_list]
def create_pop_cd(self):
cd = CellDecomposition(self.world_margin)
path_list = cd.main(self.n_ind, shortcut=True)
self.pop = [Individual(path) for path in path_list]
return self.pop
def set_fitness(self, eval_func):
"""Set fitnesses of each individual in a population."""
for i, fit in enumerate(map(eval_func, self.pop)):
self.pop[i].fitness = fit
def evalOneMax(self, gene):
"""Objective function."""
delta = [0.1]
score_list = []
for d in delta:
pts = self.smoothing(gene, d)
score = 0.0
for i in range(len(pts) - 1):
dist = np.linalg.norm(pts[i + 1] - pts[i])
score = score + dist
# if not cc.path_validation(pts[i], pts[i+1]):
# score = score + 10
if not self.ccm.path_validation(pts):
score = score + 10
score_list.append(score)
return min(score_list)
def eval_for_cx(self, gene):
"""Objective function."""
pts = gene.copy()
score = 0.0
for i in range(len(pts) - 1):
dist = np.linalg.norm(pts[i + 1] - pts[i])
score = score + dist
return score
def selTournament(self, tournsize):
"""Selection function."""
chosen = []
for i in range(self.n_ind - self.n_elite):
aspirants = [random.choice(self.pop) for j in range(tournsize)]
chosen.append(min(aspirants, key=attrgetter("fitness")))
return chosen
def selElite(self):
pop_sort = sorted(self.pop, key=attrgetter("fitness"))
elites = pop_sort[:self.n_elite]
return elites
def calc_intersection_point(self, A, B, C, D):
denominator = (B[0] - A[0]) * (C[1] - D[1]) - \
(B[1] - A[1]) * (C[0] - D[0])
# If two lines are parallel,
if abs(denominator) < 1e-6:
return None, None, None
AC = A - C
r = ((D[1] - C[1]) * AC[0] - (D[0] - C[0]) * AC[1]) / denominator
s = ((B[1] - A[1]) * AC[0] - (B[0] - A[0]) * AC[1]) / denominator
# If the intersection is out of the edges
if r < -1e-6 or r > 1.00001 or s < -1e-6 or s > 1.00001:
return None, r, s
# Endpoint and startpoint make the intersection.
if ((np.linalg.norm(r - 1.0) < 1e-6 and np.linalg.norm(s) < 1e-6) or
(np.linalg.norm(s - 1.0) < 1e-6 and np.linalg.norm(r) < 1e-6)):
return None, r, s
point_intersection = A + r * (B - A)
return point_intersection, r, s
def subcalc(self, queue, seed, offspring, elites, n):
np.random.seed(seed)
crossover = []
for i in range(10):
randint1 = np.random.randint(len(offspring))
randint2 = np.random.randint(len(elites))
child = self.cx(offspring[randint1], elites[randint2])
crossover.append(child)
queue.put(crossover)
def cx(self, ind1, ind2):
"""Crossover function for path planning."""
# --- If the ind1 and the ind2 is the same path, return ind1 and exit.
if len(ind1) == len(ind2) and all((ind1 == ind2).flatten()):
return ind1
# --- Initialize
best = []
id1 = [0]
id2 = [0]
tmp1 = ind1.copy()
tmp2 = ind2.copy()
j = 0
# --- Search for the intersection
for i1 in range(len(ind1) - 1):
for i2 in range(len(ind2) - 1):
# Calculate an intersection between line AB and line CD.
pt, r, s = self.calc_intersection_point(
ind1[i1], ind1[i1 + 1], ind2[i2], ind2[i2 + 1])
# If intersection is found,
if pt is not None:
if np.linalg.norm(r - 1.0) > 1e-6 and \
np.linalg.norm(r) > 1e-6:
# Add the intersection to the point lists.
tmp1 = np.insert(tmp1, i1 + j + 1, pt, axis=0)
tmp2 = np.insert(tmp2, i2 + j + 1, pt, axis=0)
# Revise the intersection lists.
id1.append(i1 + j + 1)
id2.append(i2 + j + 1)
# j: Num. of the intersection points.
j = j + 1
# Add the last point of the path to the intersection lists.
id1 = id1 + [len(ind1) + j + 1]
id2 = id2 + [len(ind2) + j + 1]
# --- Select the best path based on the path length.
for i in range(len(id1) - 1):
if (self.eval_for_cx(tmp1[id1[i]:id1[i + 1] + 1]) <
self.eval_for_cx(tmp2[id2[i]:id2[i + 1] + 1])):
best = best + list(tmp1[id1[i]:id1[i + 1]])
else:
best = best + list(tmp2[id2[i]:id2[i + 1]])
return Individual(best)
def node_reduction(self, path):
path = np.array(path)
new_path = [path[0]]
for i in range(1, len(path) - 1):
u = path[i + 1] - path[i]
umag = np.linalg.norm(u)
if umag > 0.1:
new_path.append(path[i])
elif not self.ccm.line_validation(new_path[-1], path[i + 1]):
new_path.append(path[i])
new_path.append(path[-1])
path = new_path.copy()
new_path = [path[0]]
for i in range(1, len(path) - 1):
u = path[i + 1] - path[i]
v = path[i - 1] - path[i]
umag = np.linalg.norm(u)
vmag = np.linalg.norm(v)
if umag != 0 and vmag != 0:
cos = np.dot(u, v) / (umag * vmag)
if abs(cos) < 0.99:
new_path.append(path[i])
elif not self.ccm.line_validation(new_path[-1], path[i + 1]):
new_path.append(path[i])
new_path.append(path[-1])
new_path = np.array(new_path)
return new_path
def mut_normal(self, ind, indpb, maxiter):
"""Mutation function."""
mut = ind.copy()
for i in range(1, len(ind) - 1):
if random.random() < indpb:
var = 0.5
for j in range(maxiter):
mut[i] = ind[i] + np.random.normal(0.0, var, 2)
var = var * 0.5
if self.ccm.path_validation(
[mut[i - 1], mut[i], mut[i + 1]]):
break
else:
mut[i] = ind[i]
return Individual(mut)
def smoothing(self, pts, delta=0.1):
# resampled_path = post_process.resampling(pts, delta)
# bezier_path = post_process.bezier(resampled_path, 50)
# return bezier_path
return pts
def main(self, duration):
'''
Main Routine for Genetic Algorithm
'''
ini_time = time.time()
# --- Evaluate the initial population
self.set_fitness(self.evalOneMax)
self.best_ind = min(self.pop, key=attrgetter("fitness"))
self.history.append([self.gtot, self.best_ind.fitness])
self.trajectory.append([self.world.start[0], self.world.start[1]])
np.vstack((self.trajectory, self.world.start))
if self.verbose:
rp.plot(self.pop, self.best_ind, self.trajectory, self.history)
# --- Generation loop starts.
print('\n[Genetic Algorithm]')
print("Generation loop start.")
print("Generation: 0. Best fitness: {}\n".format(
str(self.best_ind.fitness)))
for g in range(self.NGEN):
'''
STEP1 : Selection.
'''
t0 = time.time()
# Elite selection
elites = self.selElite()
# Tournament Selection
offspring = self.selTournament(tournsize=3)
'''
STEP2 : Mutation.
'''
t1 = time.time()
mutant = []
for ind in offspring:
if np.random.rand() < 0.7:
tmp = self.mut_normal(ind, indpb=0.7, maxiter=3)
mutant.append(tmp)
else:
mutant.append(ind)
offspring = mutant.copy()
# mutant = self.create_pop_cd(10)
mutant = self.create_pop_prm(2, 5)
'''
Step3 : Crossover.
'''
t2 = time.time()
proc = 8
n_cross = 10
queue = mp.Queue()
ps = [
mp.Process(target=self.subcalc,
args=(queue, i, offspring, elites, n_cross))
for i in np.random.randint(100, size=proc)
]
for p in ps:
p.start()
crossover = []
for i in range(proc):
crossover += queue.get()
'''
STEP4: Update next generation.
'''
t3 = time.time()
n_offspring = self.n_ind - \
(len(elites) + len(crossover) + len(mutant))
self.pop = (list(elites) + list(crossover) +
list(offspring[0:n_offspring])) + list(mutant)
# --- Delete the redundant points on the path.
self.pop = [
Individual(self.node_reduction(path)) for path in self.pop
]
self.set_fitness(self.evalOneMax)
'''
Output
'''
t4 = time.time()
# --- Print best fitness in the population.
self.best_ind = min(self.pop, key=attrgetter("fitness"))
print("Generation: {: > 2}".format(g))
print("Best fitness: {: .3f}".format(self.best_ind.fitness))
print(
"Time: {0:.3f}, {1:.3f}, {2:.3f}, {3:.3f}, Total: {4:.3f} \n".
format(t1 - t0, t2 - t1, t3 - t2, t4 - t3, t4 - t0))
# Fitness transition
self.history.append([self.gtot, self.best_ind.fitness])
'''
# STEP5: Termination
'''
# --- Visualization
if self.verbose:
rp.plot(self.pop, self.best_ind, self.trajectory, self.history)
if time.time() - ini_time > duration:
# --- Visualization
if self.verbose:
rp.plot(self.pop, self.best_ind, self.trajectory,
self.history)
break
if self.best_ind.fitness <= self.fitness_thresh:
print('The best fitness reaches the threshold value.')
break
if g >= 5:
diff = np.abs(self.history[-5][1] - self.history[-1][1])
if diff < 1e-2:
print('The best fitness does not change for 10 steps.')
break
self.gtot += 1
