class GeneticAlgorithm():
def __init__(self,
world=None,
NGEN=1000,
n_ind=100,
n_elite=10,
fitness_thresh=0.1,
margin=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:
self.world_margin = world.mcopy(rate=margin)
else:
self.world_margin = world
self.cc = CollisionChecker(world, delta=0.1)
self.ccm = CollisionChecker(self.world_margin, delta=0.1)
def create_pop_prm(self, m, n):
prm_list = [PRM(self.world_margin, 30, 3, delta=0.1) 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."""
pts = self.smoothing(gene)
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
return score
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(5):
randint = np.random.randint(len(offspring))
child1, child2 = self.oneptcx(offspring[randint], offspring)
crossover.append(child1)
crossover.append(child2)
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 oneptcx(self, ind, pop):
"""Crossover function for path planning."""
ind1 = ind.copy()
id1 = np.random.randint(len(ind1))
pt1 = ind1[id1]
pop1 = [ind for ind in pop if len(ind1) != len(ind)]
pop2 = [
ind for ind in pop
if len(ind1) == len(ind) and np.linalg.norm(ind - ind1) > 1e-8
]
pop = pop1 + pop2
min_dist = np.inf
for ind in pop:
for i, pt2 in enumerate(ind):
dist = np.linalg.norm(pt2 - pt1)
if dist < min_dist:
min_dist = dist
id2 = i
ind2 = ind
new_ind1 = np.vstack((ind1[:id1], ind2[id2:]))
new_ind2 = np.vstack((ind2[:id2], ind1[id1:]))
return Individual(new_ind1), Individual(new_ind2)
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.1
for j in range(maxiter):
mut[i] = ind[i] + np.random.normal(0.0, var, 2)
var = var * 0.5
if self.ccm.point_validation(mut[i]):
break
# 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):
pts = ferguson_spline(pts, 10)
# 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([world.start[0], world.start[1]])
np.vstack((self.trajectory, self.world.start))
if self.verbose:
rp = RealtimePlot(self.world, 100, dt=None)
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)
mutant = []
'''
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()
# crossover = []
'''
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])
draw_pop = [self.smoothing(pts) for pts in self.pop]
draw_best = self.smoothing(self.best_ind)
if self.verbose:
rp.plot(draw_pop, draw_best, self.trajectory, self.history)
'''
# STEP5: Termination
'''
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 >= 100:
diff = np.abs(self.history[-100][1] - self.history[-1][1])
if diff < 1e-2:
print('The best fitness does not change for 10 steps.')
break
self.gtot += 1
class PRM():
def __init__(self, world, n, k, delta=0.01):
# --- Collision Checker
self.cc = CollisionChecker(world, delta)
# --- World
self.world = world
# --- parameter
self.n = n
self.d = 2
self.k = k
self.V = np.array([])
self.E = []
self.roadmap = []
self.resampling_length = 0.1
self.path = None
self.smoothed_path = None
self.resampled_path = None
self.path_list = []
def create_k_edges(self, nodes):
self.E = []
for i, target_node in enumerate(nodes):
dist = [np.linalg.norm(target_node - node) for node in nodes]
indices = np.argsort(dist)
count_neighbors = 0
for j in indices[1:]:
# Collision check
if self.cc.line_validation(nodes[i], nodes[j]):
pair = tuple(sorted([i, j]))
# Duplication check
if not (pair in [e[0] for e in self.E]):
self.E.append([pair, dist[j]])
# self.E.append([pair, 1.0])
# If the number of edges equal to k, then break
count_neighbors += 1
if count_neighbors == self.k:
break
return self.E
def find_nearest_node(self, nodes, p):
mindist = np.inf
nearest_node_index = None
for i, target in enumerate(nodes):
dist = np.linalg.norm(p - target)
is_valid = self.cc.line_validation(p, target)
if mindist > dist and is_valid:
mindist = dist
nearest_node_index = i
return nearest_node_index
def create_node(self):
min = np.array([np.min(self.world.frame[:, i]) for i in range(self.d)])
max = np.array([np.max(self.world.frame[:, i]) for i in range(self.d)])
length = max - min
self.V = []
while len(self.V) < self.n:
q = np.random.rand(self.d) * length + min
if self.cc.point_validation(q):
self.V.append(q)
return np.array(self.V)
def build_roadmap(self):
# create nodes
self.V = self.create_node()
# Create edges
self.E = self.create_k_edges(self.V)
# Roadmap
self.roadmap = [
np.vstack((self.V[e[0][0]], self.V[e[0][1]])) for e in self.E
]
def query(self, start, goal):
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, start)
goal_prm = self.find_nearest_node(self.V, goal)
# If nearest node cannot be found, self.path = None
if start_prm is None or goal_prm is None:
self.path = None
# else, find shortest path (Dijkstra's algorithm)
else:
djk = Dijkstra(self.V, self.E)
djk.build(start_prm)
djk.query(goal_prm)
if djk.path is not None:
self.path = np.vstack(
(self.world.start, djk.path, self.world.goal))
else:
self.path = None
def smoothing(self, path_list):
q0 = np.array(path_list[0])
res_path_list = [q0]
i = 1
while i < len(path_list):
q1 = np.array(path_list[i])
if self.cc.line_validation(q0, q1):
i += 1
else:
q0 = np.array(path_list[i - 1])
res_path_list.append(q0)
i += 1
res_path_list.append(path_list[-1])
return np.array(res_path_list)
def resampling(self, path_list):
q0 = np.array(path_list[0])
res_position_list = [q0]
for node in path_list[1:]:
q1 = np.array(node)
vector = q1 - q0
dist = np.linalg.norm(vector)
if dist > self.resampling_length:
norm_vector = vector / dist
num_sample = int(dist / self.resampling_length)
dx = dist / num_sample
else:
norm_vector = vector
num_sample = 1
dx = 1.0
for i in range(1, num_sample + 1):
q_sample = q0 + norm_vector * dx * i
res_position_list.append(q_sample)
q0 = q1
return np.array(res_position_list)
def single_query(self, verbose=False):
iter = 0
while self.path is None:
if verbose:
t0 = time.time()
print('Trial{:>2}: Start'.format(iter))
self.build_roadmap()
if verbose:
t1 = time.time()
print('build_roadmap:{:.3f}'.format(t1 - t0))
self.query(self.world.start, self.world.goal)
if verbose:
t2 = time.time()
print('query:{:.3f}\n'.format(t2 - t1))
if iter == 10:
break
iter += 1
if self.path is not None:
self.smoothed_path = self.smoothing(self.path)
# self.resampled_path = self.resampling(self.smoothed_path)
self.path_list = [self.smoothed_path]
else:
print('Path cannot be found.')
self.path_list = []
def multi_query(self, n, verbose=False):
if verbose:
print('Start multi_query')
t0 = time.time()
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, self.world.start)
goal_prm = self.find_nearest_node(self.V, self.world.goal)
# Initialize Dijkstra module
djk1 = Dijkstra(self.V, self.E)
djk2 = Dijkstra(self.V, self.E)
# Build a distance map
djk1.build(start_prm)
djk2.build(goal_prm)
if verbose:
t1 = time.time()
print('Build a distance map: {}'.format(t1 - t0))
# Generate multiple paths
self.path_list = []
while len(self.path_list) < n:
mid_point = np.random.randint(len(self.V))
djk1.query(mid_point)
djk2.query(mid_point)
if djk1.path is not None and djk2.path is not None:
djk1.path = np.vstack((self.world.start, djk1.path))
djk2.path = np.vstack((djk2.path[-2::-1], self.world.goal))
'''
smoothed_path1 = self.smoothing(djk1.path)
smoothed_path2 = self.smoothing(djk2.path)
self.smoothed_path = np.vstack((
smoothed_path1,
smoothed_path2))
'''
self.path = np.vstack((djk1.path, djk2.path))
self.smoothed_path = self.smoothing(self.path)
# '''
self.path_list.append(self.smoothed_path)
if verbose:
t2 = time.time()
print('Generate multiple paths: {}\n'.format(t2 - t1))
class RRT():
def __init__(self, world,
branch_length=0.1, resampling_length=0.05, verbose=False):
# set RRT parameter
self.verbose = verbose
self.rrt_length = branch_length
self.resampling_length = resampling_length
self.rrt_size = 5000
self.world = world
self.path = []
self.smoothed_path = []
self.resampled_path = []
self.tree = []
self.cc = CollisionChecker(world)
def collision_check(self, nodelist):
i = 0
flag = True
q0 = np.array(nodelist[0].position)
for node in nodelist[1:]:
q1 = np.array(node.position)
if not self.cc.line_validation(q0, q1):
print('error', i)
print(q0)
print(q1)
flag = False
q0 = q1
i = i + 1
if flag:
print('No Collision')
def create_rrt_map(self):
def generate_random_position(size):
rand_array = []
for i in range(size):
min = np.min(self.world.frame[:, i])
max = np.max(self.world.frame[:, i])
rand_array.append(np.random.rand() * (max - min) + min)
return np.array(rand_array)
def find_nearest_node(nodelist, p):
mindist = float('inf')
for target in nodelist:
dist = np.linalg.norm(p - target.position)
if mindist > dist:
mindist = dist
nearest_node = target
return nearest_node
def create_new_node(index, q_rand, parent_node):
q_parent = parent_node.position
q_vector = q_rand - q_parent
dist = np.linalg.norm(q_vector)
norm_vector = q_vector / dist
q_new = q_parent + norm_vector * self.rrt_length
new_node = Node(index + 1, q_new, parent_node.index)
return new_node
def add_rrt(rrt, i):
# self.world.points = rrt
while True:
q_rand = generate_random_position(len(self.world.goal))
# world.shift(q_rand)
nearest_node = find_nearest_node(rrt, q_rand)
new_node = create_new_node(i, q_rand, nearest_node)
if self.cc.line_validation(nearest_node.position,
new_node.position):
rrt.append(new_node)
break
return rrt
def connection_check(new_node, rrt):
connect_node_candidates = []
for find_connect_node in rrt:
if self.cc.line_validation(new_node.position,
find_connect_node.position):
connect_node_candidates.append(find_connect_node)
if connect_node_candidates == []:
return None, None
else:
connect_node_1 = new_node
connect_node_2 = find_nearest_node(connect_node_candidates,
new_node.position)
return connect_node_1, connect_node_2
def connection_check2(new_node, rrt):
nearest_node = find_nearest_node(rrt, new_node.position)
if np.linalg.norm(np.array(new_node.position) -
np.array(nearest_node.position)) \
< self.rrt_length:
return new_node, nearest_node
else:
return None, None
rrt_start = [Node(0, self.world.start, 0)]
rrt_goal = [Node(0, self.world.goal, 0)]
for i in range(self.rrt_size):
# if connect node pair is found, break.
connect_node_start, connect_node_goal = \
connection_check2(rrt_start[-1], rrt_goal)
if connect_node_start is not None:
break
connect_node_goal, connect_node_start = \
connection_check2(rrt_goal[-1], rrt_start)
if connect_node_start is not None:
break
# add a new node to RRT_start
rrt_start = add_rrt(rrt_start, i)
rrt_goal = add_rrt(rrt_goal, i)
else:
print('Path cannot be found.')
return rrt_start, rrt_goal, \
connect_node_start.index, connect_node_goal.index
def path_generation(self, rrt_list, connect_index):
path_list = [rrt_list[connect_index]]
next_node = rrt_list[connect_index].parent
for node in reversed(rrt_list):
if node.index == next_node:
path_list.append(node)
next_node = node.parent
return path_list
def path_smoothing(self, path_list):
q0 = np.array(path_list[0].position)
res_path_list = [path_list[0]]
i = 1
while i < len(path_list):
q1 = np.array(path_list[i].position)
if self.cc.line_validation(q0, q1):
i += 1
else:
res_path_list.append(path_list[i - 1])
q0 = np.array(path_list[i - 1].position)
i += 1
res_path_list.append(path_list[-1])
return res_path_list
def resampling(self, path_list):
q0 = np.array(path_list[0].position)
res_position_list = [q0]
for node in path_list[1:]:
q1 = np.array(node.position)
vector = q1 - q0
dist = np.linalg.norm(vector)
if dist > self.resampling_length:
norm_vector = vector / dist
num_sample = int(dist / self.resampling_length)
dx = dist / num_sample
else:
norm_vector = vector
num_sample = 1
dx = 1.0
for i in range(1, num_sample + 1):
q_sample = q0 + norm_vector * dx * i
res_position_list.append(q_sample)
q0 = q1
return res_position_list
def rrt_visualize(self, rrt):
parent_list = [node.parent for node in rrt]
edges = [i for i in range(len(rrt)) if i not in parent_list]
path_list = []
for edge in edges:
nodeset = self.path_generation(rrt, edge)
path = np.array([node.position for node in nodeset])
path_list.append(path)
return path_list
def make_path(self):
time_0 = time.time()
print('') if self.verbose else None
print(' >>> Motion Planning Start') if self.verbose else None
# --- Get Start and Goal Point
if not self.cc.point_validation(self.world.start):
print(' error: current joint state is invalid')
return
if not self.cc.point_validation(self.world.goal):
print(' error: goal position is invalid')
return
# Rapidly Random Tree method
time_1 = time.time()
print(' >>> RRT start') if self.verbose else None
rrt_start, rrt_goal, start_connect_index, goal_connect_index = \
self.create_rrt_map()
# Path Generation
time_2 = time.time()
print(' >>> Path Generation Start ') if self.verbose else None
# --- from mid-point to start-point)
path_node_set_start = self.path_generation(
rrt_start, start_connect_index)
# --- from mid-point to goal-point)
path_node_set_goal = self.path_generation(
rrt_goal, goal_connect_index)
# --- combined path
path_node_set_start.reverse()
path_node_set = path_node_set_start + path_node_set_goal
# Smoothed Path
time_3 = time.time()
print(' >>> Smoothing Start') if self.verbose else None
smooth_path_node_set = self.path_smoothing(path_node_set)
# Re-sampling
time_4 = time.time()
print(' >>> Re-sampling Start') if self.verbose else None
position_list = self.resampling(smooth_path_node_set)
# Publishing
time_5 = time.time()
self.path = np.array([node.position for node in path_node_set])
self.smoothed_path = np.array(
[node.position for node in smooth_path_node_set])
self.resampled_path = position_list
self.tree = self.rrt_visualize(rrt_start) + \
self.rrt_visualize(rrt_goal)
print(' >>> Completed') if self.verbose else None
# Report
if self.verbose:
print('')
print(' ========== RESULTS SUMMARY ==========')
print('')
print('[start/goal]')
print('start', np.round(self.world.start, 3))
print('goal', np.round(self.world.goal, 3))
print('')
print('[RRT]')
print('#RRT node: ', len(rrt_start), '/', len(rrt_goal))
print('')
print('[trajectory points]')
for node in smooth_path_node_set:
print(np.round(node.position, 3))
self.collision_check(smooth_path_node_set)
print('')
print('[Re-Sampling Segment]')
count = 1
seg = 1
length0 = np.linalg.norm(position_list[1] - position_list[0])
for i in range(len(position_list) - 1):
length1 = np.linalg.norm(
position_list[i + 1] - position_list[i])
if np.round(length1, 8) == np.round(length0, 8) \
and i != len(position_list) - 2:
count += 1
else:
print('seg' + str(seg), length0, count)
seg += 1
count = 1
length0 = length1
print('')
print('[time]')
print('Set Goal : ', time_1 - time_0)
print('RRT : ', time_2 - time_1)
print('Path : ', time_3 - time_2)
print('Smoothing: ', time_4 - time_3)
print('Re-sample: ', time_5 - time_4)
print('---------------------------')
print('TOTAL : ', time_5 - time_0)
