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 __init__(self): self.world_size = GlobalVars.get_world_size() self.bird_state = BirdState() self.background_state = BackgroundState() self.collision_checker = CollisionChecker(self.background_state, self.bird_state) self.is_alive = True self.score = 0
class TrajectoryGenerator(object): """ Makes calls to the motion planner and checks the resulting trajectory for collisions. """ def __init__(self, env): self.env = env self.robot = self.env.GetRobots()[0] self.manip = self.robot.GetActiveManipulator() self.motion_planner = TrajoptPlanner(self.env) self.collision_checker = CollisionChecker(self.env) def traj_from_pose(self, pos, rot, collisionfree=True, joint_targets=None, n_steps=None): traj = self.motion_planner.plan_with_pose(pos, rot, collisionfree, joint_targets, n_steps) collisions = self.collision_checker.get_collisions(traj) if collisionfree and collisions: return None, collisions else: return traj, collisions def traj_from_joints(self, joint_targets, collisionfree=True, n_steps=None): traj = self.motion_planner.plan_with_joints(joint_targets, collisionfree, n_steps) collisions = self.collision_checker.get_collisions(traj) if collisionfree and collisions: return None, collisions else: return traj, collisions
class Arena(object): def __init__(self, width, height): self.width = width self.height = height self.collision_checker = CollisionChecker(self) self.stuff = {} def agents(self): result = list(self.stuff.keys()) result.append(self.collision_checker) return result def place(self, something, x, y): """place something on the arena""" x %= self.width y %= self.height if x > self.width: raise ArenaError("%s is too high for x (0 - %s)" % (x, self.width)) if y > self.height: raise ArenaError("%s is too high for y (0 - %s)" % (y, self.height)) self.stuff[something] = {'coordinates': (x, y)} def remove(self, something): try: del self.stuff[something] self.collision_checker.forget(something) except KeyError: pass def where_is(self, something): return self.stuff[something]['coordinates'] def neighbours_of(self, centre, distance, _type): x, y = self.stuff[centre]['coordinates'] distance_2 = distance**2 for thing, data in self.stuff.iteritems(): if not isinstance(thing, _type): continue dx = x - data['coordinates'][0] dy = y - data['coordinates'][1] if dx**2 + dy**2 > distance_2: continue yield thing def move(self, thing, dx, dy): x, y = self.stuff[thing]['coordinates'] x, y = x + dx, y + dy x = x % self.width y = y % self.height self.stuff[thing]['coordinates'] = (x, y) def __repr__(self): return "Arena"
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 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)
def __init__(self, width, height, n_asteroids, min_mass=800, max_mass=1000): self.min_mass = min_mass self.max_mass = max_mass self.schedule = Schedule('move', 'random', 'collision_check', 'collision_response') Asteroid.activate(self.schedule) Ship.activate(self.schedule) Bullet.activate(self.schedule) CollisionChecker.activate(self.schedule) self._tick = 0 self.logger = logging.getLogger("Model") self.arena = Arena(width, height) for i in range(n_asteroids): self.add_asteroid() centre = [self.arena.width / 2, self.arena.height / 2] self.ship = Ship(centre, 1500, 150, 1500, self.arena)
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 __init__(self, num_paths, path_offset, circle_offsets, circle_radii, path_select_weight, time_gap, a_max, coasting_speed, stop_line_buffer): self.num_paths = num_paths self.path_offset = path_offset self.path_optimizer = PathOptimizer() self.collision_checker = CollisionChecker(circle_offsets, circle_radii, path_select_weight) self.velocity_planner = VelocityPlanner(time_gap, a_max, coasting_speed, stop_line_buffer)
def __init__( self, road: Road, actions: Actions, constraints: Constraints, termination_conditions: TerminationConditions, cost_calculator: CostCalculator, start_state: State, ego, subject, ): self.road = road self.actions = actions self.start_state = start_state self.constraints = constraints self.termination_conditions = termination_conditions self.cost_calculator = cost_calculator self.collision_checker = CollisionChecker(1.75, 1) self.ego_vehicle = ego self.subject_vehicle = subject
def locate(self, scale_minmax): frame = np.array(self.world.frame) xmax = np.max(frame[:, 0]) xmin = np.min(frame[:, 0]) ymax = np.max(frame[:, 1]) ymin = np.min(frame[:, 1]) def collision_check(target): res = [ cc.path_validation_new(target[i], target[i + 1]) for i in range(len(target) - 1) ] res_start = [ cc.crossing_number_algorithm(self.world.start, target) ] res_goal = [cc.crossing_number_algorithm(self.world.goal, target)] return all(res + res_start + res_goal) new_objects = [] for obj in self.world.objects: self.world.objects = new_objects cc = CollisionChecker(self.world, 0.05) for iter in range(10): # set random value # x, y: translate distance, theta: rotate angle, scale: scale x = np.random.rand() * (xmax - xmin) + xmin y = np.random.rand() * (ymax - ymin) + ymin theta = np.random.rand() * 2.0 * pi scale = np.random.rand() * \ (scale_minmax[1] - scale_minmax[0]) + scale_minmax[0] # Rotate, scale, translate rot_mat = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) rslt = np.dot(rot_mat, obj.T) * scale + \ np.array([x, y]).reshape(-1, 1) # Check collisions with the other objects. if collision_check(rslt.T): new_objects.append(rslt.T) break self.world.objects = new_objects.copy()
class GameState: def __init__(self): self.world_size = GlobalVars.get_world_size() self.bird_state = BirdState() self.background_state = BackgroundState() self.collision_checker = CollisionChecker(self.background_state, self.bird_state) self.is_alive = True self.score = 0 def update(self, move_command): self.bird_state.update(move_command) self.background_state.update() if self.collision_checker.check_collision(): self.is_alive = False def get_bird_position(self): return self.bird_state.bird_position
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))
def __init__(self, width, height): self.width = width self.height = height self.collision_checker = CollisionChecker(self) self.stuff = {}
world = World() # --- Set frame world.frame = np.array([[-pi, -pi], [-pi, pi], [pi, pi], [pi, -pi], [-pi, -pi]]) # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 # --- Generate objects og = ObjectGenerator(world) world.objects = og.example() # --- CollisionChecker cc = CollisionChecker(world) # In[]: m = 6 n = 6 w = 1.0 h = 1.0 x0 = -3.0 y0 = -3.0 V = [[x0 + i * w, y0 + j * h] for j in range(n) for i in range(m)] E = [] for i in range(m * n - 1): if i % m != (m - 1): E.append([(i, i + 1), 1.0])
class LatticeGenerator: def __init__( self, road: Road, actions: Actions, constraints: Constraints, termination_conditions: TerminationConditions, cost_calculator: CostCalculator, start_state: State, ego, subject, ): self.road = road self.actions = actions self.start_state = start_state self.constraints = constraints self.termination_conditions = termination_conditions self.cost_calculator = cost_calculator self.collision_checker = CollisionChecker(1.75, 1) self.ego_vehicle = ego self.subject_vehicle = subject def sample_random_trajectory(self): tmp_state = copy.deepcopy(self.start_state) trajectory = [self.start_state] ALLOW_LANE_CHANGE = True while True: # 1. Randomly select an action is_next_action_lane_change, next_action_params = self.actions.random_action( ALLOW_LANE_CHANGE) if is_next_action_lane_change: ALLOW_LANE_CHANGE = False # 2. Apply action + constraints # 2.a T tmp_state.time += next_action_params["deltaT"] # 2.b D tmp_state.position[0] += (tmp_state.speed + next_action_params["deltaDDiff"][0]) tmp_state.position[1] += next_action_params["deltaDDiff"][1] tmp_state.apply_constraints(self.constraints) # 2.c V tmp_state.speed += next_action_params["deltaV"] tmp_state.apply_constraints(self.constraints) # 3. Append to trajectory trajectory.append(copy.deepcopy(tmp_state)) # 4. Check if terminal if tmp_state.check_termination(self.termination_conditions): break return trajectory def sample_and_plot_random_trajectory(self): random_trajectory = self.sample_random_trajectory() # Plot the road self.road.plot() # Plot the states of the random trajectory x_vals = [] y_vals = [] for state in random_trajectory: plt.plot(state.position[0], state.position[1], "o") x_vals.append(state.position[0]) y_vals.append(state.position[1]) plt.plot(x_vals, y_vals) return random_trajectory def check_dense_collision_for_lane_change( self, start_state, end_state, subject_path, action_params, ego_size, subject_size, num_points=10, ): start_x = start_state.position[0] start_y = start_state.position[1] end_x = end_state.position[0] end_y = end_state.position[1] x_points = np.linspace(start_x, end_x, num_points) y_points = np.linspace(start_y, end_y, num_points) time_points = np.linspace(start_state.time, end_state.time, num_points) for i in range(len(time_points)): intermediate_state = State( position=[x_points[i], y_points[i]], speed=start_state.speed, time=time_points[i], ) if self.collision_checker.predict_collision( ego_size, subject_size, intermediate_state, subject_path, True): return True return False def generate_full_lattice(self, start_state, subject_path, lane_change_init_flag): lattice = ( {} ) # (x,y,v,t,lane_change_status) -> {"action" -> next (x,y,v,t,lane_change_status)} state_2_cost = {} goal_cost = 2**31 queue = PriorityQueue() queue.put((0, lane_change_init_flag, start_state)) # (state, has lane change happened, cost) goal_state = None while not queue.empty(): cost, has_lane_change_happend, curr_state = queue.get() curr_state_tuple = ( curr_state.position[0], curr_state.position[1], curr_state.speed, curr_state.time, has_lane_change_happend, ) if (curr_state_tuple in lattice.keys() and state_2_cost[curr_state_tuple] <= cost): continue lattice[curr_state_tuple] = {} state_2_cost[curr_state_tuple] = cost # Find best goal state if (has_lane_change_happend and curr_state.position[0] > 100 # and cost < goal_cost ): goal_state = curr_state_tuple goal_cost = cost break for i, next_action_params in enumerate( self.actions.action_params_list): if i == 3 and has_lane_change_happend: continue tmp_state = copy.deepcopy(curr_state) if i == 2 and tmp_state.speed < 8: continue # Apply action + constraints # a T tmp_state.time += next_action_params["deltaT"] # b D if i != 3: tmp_state.position[0] += ( tmp_state.speed + next_action_params["deltaDDiff"][0]) else: tmp_state.position[0] += tmp_state.speed * 3.6 tmp_state.position[1] += next_action_params["deltaDDiff"][1] tmp_state.apply_constraints(self.constraints) # c V tmp_state.speed += next_action_params["deltaV"] tmp_state.apply_constraints(self.constraints) # Check collision ego_size = [ self.ego_vehicle.bounding_box.extent.x, self.ego_vehicle.bounding_box.extent.y, ] subject_size = [ self.subject_vehicle.bounding_box.extent.x, self.subject_vehicle.bounding_box.extent.y, ] if i == 3: if self.check_dense_collision_for_lane_change( curr_state, tmp_state, subject_path, next_action_params, ego_size, subject_size, ): print("Dense Collision detected....................") continue else: if self.collision_checker.predict_collision( ego_size, subject_size, tmp_state, subject_path, i == 3): print("Lane Collision detected....................") continue if i == 3 or has_lane_change_happend: tmp_state_tuple = ( tmp_state.position[0], tmp_state.position[1], tmp_state.speed, tmp_state.time, 1, ) else: tmp_state_tuple = ( tmp_state.position[0], tmp_state.position[1], tmp_state.speed, tmp_state.time, 0, ) lattice[curr_state_tuple][i] = tmp_state_tuple cost_of_transition = self.cost_calculator.compute_transition_cost( next_action_params, curr_state, tmp_state, subject_path) # Check if terminal if tmp_state.check_termination( self.termination_conditions) and i != 3: continue # Add to queue if i == 3 or has_lane_change_happend: queue.put((cost + cost_of_transition, 1, tmp_state)) else: queue.put((cost + cost_of_transition, 0, tmp_state)) ## Reverse Direction Lattice (x,y,v,t,lane_change_status) -> {"action" -> parent (x,y,v,t,lane_change_status)} reverse_lattice = {} for parent_state in lattice.keys(): for action in lattice[parent_state].keys(): next_state = lattice[parent_state][action] if next_state in reverse_lattice.keys(): reverse_lattice[next_state][action] = parent_state else: reverse_lattice[next_state] = {action: parent_state} # import ipdb # ipdb.set_trace() return lattice, state_2_cost, reverse_lattice, goal_state def backtrack_from_state(self, reverse_lattice, state_2_cost, end_state_tuple, start_state_tuple): curr_state_tuple = copy.deepcopy(end_state_tuple) path = [curr_state_tuple] action_sequence = [] while curr_state_tuple != start_state_tuple: minCostAction = -1 minCost = pow(10, 9) # Some large number for possible_action in reverse_lattice[curr_state_tuple].keys(): corresponding_parent_state_tuple = reverse_lattice[ curr_state_tuple][possible_action] if state_2_cost[corresponding_parent_state_tuple] < minCost: minCost = state_2_cost[corresponding_parent_state_tuple] minCostAction = possible_action curr_state_tuple = reverse_lattice[curr_state_tuple][minCostAction] path.append(curr_state_tuple) action_sequence.append(minCostAction) path.reverse() action_sequence.reverse() return path, action_sequence def plot_some_trajectories(self, lattice, start_state_tuple, k=10): self.road.plot() for i in range(k): curr_state_tuple = copy.deepcopy(start_state_tuple) traj = [curr_state_tuple] while curr_state_tuple in lattice.keys(): next_action = random.choice( [k for k in range(len(lattice[curr_state_tuple]))]) next_state_tuple = lattice[curr_state_tuple][next_action] traj.append(next_state_tuple) curr_state_tuple = copy.deepcopy(next_state_tuple) x_vals = [p[0] for p in traj] y_vals = [p[1] for p in traj] plt.plot(x_vals, y_vals) for i in range(len(x_vals)): plt.plot(x_vals[i], y_vals[i], "o") return 0
global sensor_data sensor_data = data if __name__ == '__main__': os.environ["WANDB_SILENT"] = "true" # Avoid small wandb bug # Subscribe to essential pathfinding info sailbot = sbot.Sailbot(nodeName='log_stats') rospy.Subscriber("sensors", msg.Sensors, sensorsCallback) sailbot.waitForFirstSensorDataAndGlobalPath() # Setup subscribers log_closest_obstacle = LogClosestObstacle(create_ros_node=False) path_storer = PathStorer(create_ros_node=False) collision_checker = CollisionChecker(create_ros_node=False) sailbot_gps_data = SailbotGPSData(create_ros_node=False) rate = rospy.Rate(UPDATE_TIME_SECONDS) # Set wandb run name and directory path start_datetime = datetime.now().strftime('%b_%d-%H_%M_%S') run_name = 'stats-' + start_datetime dir_dir = os.path.expanduser('~/.ros/stats/') dir_path = dir_dir + start_datetime os.makedirs(dir_dir + start_datetime) os.symlink(dir_path, dir_dir + 'tmp') os.rename(dir_dir + 'tmp', dir_dir + 'latest-dir') # use replace() in Python 3 wandb.init(entity='sailbot', project='stats', dir=dir_path, name=run_name)
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
''' World setting ''' # --- world class world = World() # --- Set frame world.frame = np.array( [[-pi, -pi], [-pi, pi], [pi, pi], [pi, -pi], [-pi, -pi]]) # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 # --- Generate objects og = ObjectGenerator(world) world.objects = og.example() world_margin = world.mcopy(rate=1.1) cc = CollisionChecker(world_margin) # In[]: # --- Visualize gd = GraphDrawer(world_margin) plt.show()
def __init__(self, env): self.env = env self.robot = self.env.GetRobots()[0] self.manip = self.robot.GetActiveManipulator() self.motion_planner = TrajoptPlanner(self.env) self.collision_checker = CollisionChecker(self.env)
[pi, -pi], [-pi, -pi]]) # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 # --- Generate objects og = ObjectGenerator(world) world.objects = og.example() # og.generate(10, 5) # og.locate([2.0, 3.0]) # world.objects = og.world.objects # --- Generte Collision Checker cc = CollisionChecker(world, 0.1) ''' Functions ''' class Individual(np.ndarray): """Container of a individual.""" fitness = None def __new__(cls, a): return np.asarray(a).view(cls) def create_pop_prm(m, n):
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)