def find_graph_path(spec, init_node): """ This method performs a breadth first search of the state graph and return a list of configs which form a path through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list. You may use this method in your solver if you wish, or can implement your own graph search algorithm to improve performance. :param spec: ProblemSpec object :param init_node: GraphNode object for the initial configuration :return: List of configs forming a path through the graph from initial to goal """ # search the graph init_container = [init_node] # here, each key is a graph node, each value is the list of configs visited on the path to the graph node init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) if test_config_equality(current.config, spec.goal, spec): # found path to goal return init_visited[current] successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config] return None
def solve(spec, output): """ An example solve method containing code to perform a breadth first search of the state graph and return a list of configs which form a path through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list. If you wish to use this code, you may either copy this code into your own file or add your existing code to this file. :param spec: ProblemSpec object :return: List of configs forming a path through the graph from initial to goal """ init_node = GraphNode(spec, spec.initial) goal_node = GraphNode(spec, spec.goal) # TODO: Insert your code to build the state graph here # *** example for adding neighbors *** # if path between n1 and n2 is collision free: # n1.neighbors.append(n2) # n2.neighbors.append(n1) vertices, edges = state_graph(350, 45, 5, init_node, goal_node, spec) # search the graph init_container = [init_node] # here, each key is a graph node, each value is the list of configs visited on the path to the graph node init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) # print(current.__hash__()) if test_config_equality(current.config, spec.goal, spec): # found path to goal # print(init_visited[current]) print("found") # for i in init_visited[current]: # print(str(i)) write_robot_config_list_to_file(output, primitive_step(init_visited[current])) print("finish") return init_visited[current] successors = current.get_successors() for suc in successors: if suc not in init_visited: if test_self_collision(suc.config, spec) \ and test_environment_bounds(suc.config) \ and test_obstacle_collision(suc.config, spec, spec.obstacles): init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config]
def path_between_points(initial, goal, spec, number_of_iterations,a_count): init_node = GraphNode(spec, initial) goal_node = GraphNode(spec, goal) GraphNodes = [] # List of all nodes created found_result = False for i in range(number_of_iterations): print("iteration number: ", i) # Create Random Samples random_graph_nodes = randomsample([init_node, goal_node], 100, spec, GraphNodes,a_count) # Building state graph init_container = [init_node] init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) if test_config_equality(current.config, goal, spec): # found path to goal print("Finally :)") final_config = init_visited[current] # Build small neighbours print_configs = [] for i in range(len(final_config) - 1): neigh = neighbour_check_configs(final_config[i], final_config[i + 1], spec) if neigh: for j in neigh: print_configs.append(j) else: print("Something is just not right") print_configs.append(final_config[len(final_config) - 1]) return print_configs # Check each successor to check if previously visited node successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config] return None
def solve(spec): """ An example solve method containing code to perform a breadth first search of the state graph and return a list of configs which form a path through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list. If you wish to use this code, you may either copy this code into your own file or add your existing code to this file. :param spec: ProblemSpec object :return: List of configs forming a path through the graph from initial to goal """ init_node = GraphNode(spec, spec.initial) goal_node = GraphNode(spec, spec.goal) # TODO: Insert your code to build the state graph here # *** example for adding neighbors *** # if path between n1 and n2 is collision free: # n1.neighbors.append(n2) # n2.neighbors.append(n1) # search the graph init_container = [init_node] # here, each key is a graph node, each value is the list of configs visited on the path to the graph node init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) if test_config_equality(current.config, spec.goal, spec): # found path to goal return init_visited[current] successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config]
def __eq__(self, other): return test_config_equality(self.config, other.config, self.spec)
def create_state_graph(spec, obstacles): init_node = GraphNode(spec, spec.initial) goal_node = GraphNode(spec, spec.goal) nodes = [init_node, goal_node] for stage in range(spec.num_grapple_points): for i in range(500): sample_point = uniformly_sampling(spec, obstacles, stage) new_node = GraphNode(spec, sample_point) nodes.append(new_node) for node in nodes: if check_distance( spec, sample_point, node.config) and check_path( sample_point, node.config, spec, obstacles): node.neighbors.append(new_node) new_node.neighbors.append(node) init_container = [init_node] init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) if test_config_equality(current.config, spec.goal, spec): result = init_visited[current] # return result new_list = [] for i in range(len(result) - 1): angles1 = result[i].ee1_angles angles2 = result[i + 1].ee1_angles if result[i].ee2_grappled is True: angles1 = result[i].ee2_angles angles2 = result[i + 1].ee2_angles angle_distance = [] lengths1 = result[i].lengths lengths2 = result[i + 1].lengths length_distance = [] for j in range(len(angles1)): angle_distance.append(angles2[j].__sub__(angles1[j])) length_distance.append(lengths2[j] - lengths1[j]) for j in range(501): new_angle_list = [] new_length_list = [] for k in range(len(angles1)): if result[i].ee1_grappled is True: new_angle_list.append( result[i].ee1_angles[k].__add__( angle_distance[k].__mul__(0.002 * j))) elif result[i].ee2_grappled is True: new_angle_list.append( result[i].ee2_angles[k].__add__( angle_distance[k].__mul__(0.002 * j))) new_length_list.append(result[i].lengths[k] + length_distance[k] * 0.002 * j) if result[i].ee1_grappled is True: x, y = result[i].points[0] new_config = make_robot_config_from_ee1( x, y, new_angle_list, new_length_list, ee1_grappled=True, ee2_grappled=False) if test_obstacle_collision( new_config, spec, obstacles) and test_self_collision( new_config, spec ) and test_environment_bounds(new_config): new_list.append(new_config) elif result[i].ee2_grappled is True: x, y = result[i].points[-1] new_config = make_robot_config_from_ee2( x, y, new_angle_list, list(reversed(new_length_list)), ee1_grappled=False, ee2_grappled=True) if test_obstacle_collision( new_config, spec, obstacles) and test_self_collision( new_config, spec ) and test_environment_bounds(new_config): new_list.append(new_config) return new_list successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config]
def create_state_graph(spec, obstacles, stage, sub_init=None, sub_goals=None): if sub_init is None: init_node = GraphNode(spec, spec.initial) else: init_node = GraphNode(spec, sub_init) nodes = [init_node] if sub_goals is None: goal_node = GraphNode(spec, spec.goal) nodes.append(goal_node) else: for sub_goal in sub_goals: nodes.append(GraphNode(spec, sub_goal)) for i in range(2000): sample_point = uniformly_sampling(spec, obstacles, stage) new_node = GraphNode(spec, sample_point) nodes.append(new_node) for node in nodes: if check_distance(spec, sample_point, node.config, stage): if check_path(sample_point, node.config, spec, obstacles, stage): node.neighbors.append(new_node) new_node.neighbors.append(node) init_container = [init_node] init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) find_goal = False if sub_goals is None: if test_config_equality(current.config, spec.goal, spec): find_goal = True else: for sub_goal in sub_goals: if test_config_equality(current.config, sub_goal, spec): find_goal = True if find_goal is True: result = init_visited[current] # return result new_list = [] for i in range(len(result) - 1): angles1 = result[i].ee1_angles angles2 = result[i + 1].ee1_angles if stage % 2 == 1: angles1 = result[i].ee2_angles angles2 = result[i + 1].ee2_angles angle_distance = [] lengths1 = result[i].lengths lengths2 = result[i + 1].lengths length_distance = [] for j in range(len(angles1)): angle_distance.append(angles2[j].__sub__(angles1[j])) length_distance.append(lengths2[j] - lengths1[j]) for j in range(501): new_angle_list = [] new_length_list = [] for k in range(len(angles1)): if stage % 2 == 0: new_angle_list.append(result[i].ee1_angles[k].__add__(angle_distance[k].__mul__(0.002 * j))) elif stage % 2 == 1: new_angle_list.append(result[i].ee2_angles[k].__add__(angle_distance[k].__mul__(0.002 * j))) new_length_list.append(result[i].lengths[k] + length_distance[k] * 0.002 * j) if stage % 2 == 0: x, y = result[i].points[0] new_config = make_robot_config_from_ee1(x, y, new_angle_list, new_length_list, ee1_grappled=True) new_list.append(new_config) elif stage % 2 == 1: x, y = result[i].points[-1] new_config = make_robot_config_from_ee2(x, y, new_angle_list, new_length_list, ee2_grappled=True) new_list.append(new_config) return new_list successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config]
def find_graph_path(spec, obstacles, phase, partial_init=None, bridge_configs=None): """ This method performs a breadth first search of the state graph and return a list of configs which form a path through the state graph between the initial and the goal. Note that this path will not satisfy the primitive step requirement - you will need to interpolate between the configs in the returned list. :param spec: ProblemSpec object :param obstacles: Obstacles object :return: List of configs forming a path through the graph from initial to goal for i := range[1, N] do configs = uniformly sampling(); for config in all configs do if distance between c1 and c2 is below certain amount and path is valid do add a neighbor relationship between nodes; search graph if goal reached: break """ # set init and goal nodes, put into nodes list init_node = GraphNode(spec, spec.initial) if partial_init is None else GraphNode( spec, partial_init) nodes = [init_node] if bridge_configs is None: goal_node = GraphNode(spec, spec.goal) nodes.append(goal_node) else: for partial_goal in bridge_configs: nodes.append(GraphNode(spec, partial_goal)) # sample configs sample_number = 1000 if spec.num_segments <= 3 else 2000 for i in range(sample_number): sample_point = uniformly_sampling(spec, obstacles, phase) new_node = GraphNode(spec, sample_point) nodes.append(new_node) for node in nodes: if distance_checking(spec, phase, sample_point, node.config) \ and interpolation_checking(spec, phase, sample_point, node.config, obstacles): GraphNode.add_connection(node, new_node) # search the graph init_container = [init_node] # here, each key is a graph node, each value is the list of configs visited on the path to the graph node init_visited = {init_node: [init_node.config]} while len(init_container) > 0: current = init_container.pop(0) complete_find = False if bridge_configs is None: if test_config_equality(current.config, spec.goal, spec): complete_find = True else: for partial_goal in bridge_configs: if test_config_equality(current.config, partial_goal, spec): complete_find = True if complete_find is True: # found path to goal config_list = init_visited[current] path_list = [] for n in range(len(config_list) - 1): angles_first = config_list[ n].ee1_angles if phase % 2 == 0 else config_list[ n].ee2_angles angles_sec = config_list[ n + 1].ee1_angles if phase % 2 == 0 else config_list[ n + 1].ee2_angles between_angle = [] between_lengths = [] lengths_first = config_list[n].lengths lengths_sec = config_list[n + 1].lengths for i in range(len(angles_first)): between_angle.append(angles_sec[i].__sub__( angles_first[i])) between_lengths.append(lengths_sec[i] - lengths_first[i]) # interpolated the path into steps less than 0,001 for j in range(851): valid_angle_list = [] valid_length_list = [] for k in range(len(angles_first)): if phase % 2 == 0: valid_angle_list.append( config_list[n].ee1_angles[k].__add__( between_angle[k].__mul__(1 / 850 * j))) else: valid_angle_list.append( config_list[n].ee2_angles[k].__add__( between_angle[k].__mul__(1 / 850 * j))) valid_length_list.append(config_list[n].lengths[k] + between_lengths[k] * 1 / 850 * j) if phase % 2 == 0: x, y = config_list[n].points[0] new_config = make_robot_config_from_ee1( x, y, valid_angle_list, valid_length_list, ee1_grappled=True) path_list.append(new_config) else: x, y = config_list[n].points[-1] new_config = make_robot_config_from_ee2( x, y, valid_angle_list, valid_length_list, ee2_grappled=True) path_list.append(new_config) return path_list successors = current.get_successors() for suc in successors: if suc not in init_visited: init_container.append(suc) init_visited[suc] = init_visited[current] + [suc.config]
def interpolate_path(RobotConfig1, RobotConfig2): paths = [RobotConfig1] lengths1 = RobotConfig1.lengths lengths2 = RobotConfig2.lengths kLengths = [] for i in range(len(lengths1)): differences = lengths2[i] - lengths1[i] if differences >= 0: kLengths.append(math.floor(differences / 0.001)) else: kLengths.append(math.ceil(differences / 0.001)) angles1 = RobotConfig1.ee1_angles angles2 = RobotConfig2.ee1_angles kAngles = [] for i in range(len(angles1)): differences = angles2[i].radians - angles1[i].radians if differences >= 0: kAngles.append(math.floor(differences / 0.001)) else: kAngles.append(math.ceil(differences / 0.001)) if RobotConfig1.ee1_grappled: ee2Grappled = True if RobotConfig1.ee2_grappled else False ee1Grappled = True grappledX, grappledY = RobotConfig1.get_ee1() else: ee2Grappled = True ee1Grappled = False grappledX, grappledY = RobotConfig1.get_ee2() # for each config, newLengths = [] newAngles = [] counter = 0 # getConfig2 = False while True: if counter < 2: counter += 1 for i in range(len(lengths1)): if kLengths[i] > 0: if counter == 1: newLengths.append(lengths1[i] + 0.001) else: newLengths[i] += 0.001 kLengths[i] -= 1 elif kLengths[i] < 0: if counter == 1: newLengths.append(lengths1[i] - 0.001) else: newLengths[i] -= 0.001 kLengths[i] += 1 else: if counter != 1: if lengths2[i] - newLengths[i] > 0: newLengths[i] += (lengths2[i] - newLengths[i]) elif lengths2[i] - newLengths[i] < 0: newLengths[i] -= (lengths1[i] - lengths2[i]) else: newLengths.append(lengths1[i]) if kAngles[i] > 0: if counter == 1: newAngles.append(angles1[i] + 0.001) else: newAngles[i] += 0.001 kAngles[i] -= 1 print(newAngles[i]) print(angles2[i]) elif kAngles[i] < 0: if counter == 1: newAngles.append(angles1[i] - 0.001) else: newAngles[i] -= 0.001 kAngles[i] += 1 else: if counter != 1: # if i == 0: # print("This") if angles2[i] > newAngles[i]: newAngles[i] += (angles2[i] - newAngles[i]) elif angles2[i] < newAngles[i]: newAngles[i] -= (newAngles[i] - angles2[i]) else: newAngles.append(angles1[i]) if i == len(lengths1) - 1: newConfig = make_robot_config_from_ee1(x=grappledX, y=grappledY, angles=newAngles.copy(), lengths=newLengths.copy(), ee1_grappled=ee1Grappled, ee2_grappled=ee2Grappled) paths.append(newConfig) if paths[-1]: if test_config_equality(paths[-1], RobotConfig2, spec): break return paths