def shortcut(path_list, world): cc = CollisionChecker(world) q0 = np.array(path_list[0]) res_path_list = [q0] i = 1 while i < len(path_list): q1 = np.array(path_list[i]) if 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)
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
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)