def find_forward_trajectory(idx=0): start_node = Node(START_LINE[idx][0], START_LINE[idx][1], 0, 0) start_key = start_node.key state = graph[start_key] forward_trajectory = [state] while not state.is_goal: value_uk_9 = [] value_uk_1 = [] for child_idx in range(len(ACTION_SPACE)): child_key_9 = state.next_prob_9[child_idx] child_9 = graph[child_key_9] child_key_1 = state.next_prob_1[child_idx] child_1 = graph[child_key_1] value_uk_9.append(child_9.g_value) value_uk_1.append(child_1.g_value) if np.min(value_uk_1) > np.min(value_uk_9): child_key = state.next_prob_9[np.argmin(value_uk_9)] else: child_key = state.next_prob_1[np.argmin(value_uk_1)] if random() > 0.9: child_key = state.next_prob_9[randint(0, len(value_uk_9) - 1)] state = graph[child_key] forward_trajectory.append(state) return forward_trajectory
def generate_trajectory(iter_num, idx=1): start_node = Node(START_LINE[idx][0], START_LINE[idx][1], 0, 0) start_key = start_node.key state = graph[start_key] goal_position = np.asarray(FINISH_LINE[idx]) trajectory = [state] while not state.is_goal: value_uk = [] for child_idx in range(len(ACTION_SPACE)): child_key_9 = state.next_prob_9[child_idx] child_9 = graph[child_key_9] if child_9.g_value == 0: child_9.g_value = initialize_graph_value( goal_position, child_9) value_uk.append(child_9.g_value) random_prob = 0.4 * np.exp(-iter_num * 0.01) if np.random.choice([0, 1], p=[1 - random_prob, random_prob]): child_key = state.next_prob_9[np.random.choice(len(value_uk))] else: child_key = state.next_prob_9[np.argmin(value_uk)] state = graph[child_key] trajectory.append(state) trajectory.reverse() return trajectory # def Real_time_dynamic_programming(): """ update value without concern about action space
def loader(path): with open(path, 'r') as JSON_file: original = JSON_file.read() reformed = reform(original) data = json.loads(reformed)['pens'] node_list = [] curve_list = [] for i in range(len(data)): tmp_data = data[i] if tmp_data['type'] == 0: node_list.append(Node(tmp_data['id'], tmp_data['name'], tmp_data['text'], tmp_data['data'])) elif tmp_data['type'] == 1: curve_list.append(Curve(tmp_data['id'], tmp_data['name'], tmp_data['from']['id'], tmp_data['to']['id'])) id2block = {} for i in range(len(node_list)): id2block[node_list[i].n_id] = node_list[i] print(node_list[i]) for i in range(len(curve_list)): from_block = id2block[curve_list[i].from_id] to_block = id2block[curve_list[i].to_id] from_block.output.append(to_block) to_block.input.append(from_block) return node_list
def track_the_best_plan(): start_node = Node(START_LINE[idx][0], START_LINE[idx][1], 0, 0) start_key = start_node.key state = graph[start_key] trajectory = [state] # for i in range(grid.shape[0]+grid.shape[1]) a safer condition while not state.is_goal: value_uk = [] for child_idx in range(len(ACTION_SPACE)): child_key_9 = state.next_prob_9[child_idx] child_9 = graph[child_key_9] value_uk.append(child_9.g_value) child_key = state.next_prob_9[np.argmin(value_uk)] state = graph[child_key] trajectory.append(state) print(state.px, state.py) return trajectory
def generate(self, min_nodes, max_nodes, min_edges, max_edges, min_xy, max_xy): self.graph = Graph() self.nodes_count = randint(min_nodes, max_nodes) self.edges_count = randint(min_edges, max_edges) self.edges_count = min(self.edges_count, 3 * self.nodes_count - 6) self.edges_count = max(self.edges_count, 0) for i in range(self.nodes_count): self.graph.add_node( Node(i, randint(min_xy, max_xy), randint(min_xy, max_xy))) for i in range(self.edges_count): edge = self._get_random_valid_edge() if edge: self.graph.add_edge(edge) else: break return self.graph
def generate_circular(self, min_nodes, max_nodes, min_edges, max_edges, radius): self.graph = Graph() self.nodes_count = randint(min_nodes, max_nodes) self.edges_count = randint(min_edges, max_edges) self.edges_count = min(self.edges_count, 3 * self.nodes_count - 6) self.edges_count = max(self.edges_count, 0) for i in range(self.nodes_count): theta = i / self.nodes_count * 2 * pi x, y = int(radius * cos(theta)), int(radius * sin(theta)) self.graph.add_node(Node(i, x, y)) for i in range(self.edges_count): edge = self._get_random_valid_edge() if edge: self.graph.add_edge(edge) else: break return self.graph
def greedy_policy(idx = 0, explore = True): start_node = Node(START_LINE[idx][0], START_LINE[idx][1],0,0) start_key = start_node.key start = graph[start_key] trajectory = [state.key] while not state.is_goal: value_uk = [] for child_idx in range(len(ACTION_SPACE)): child_key_9 = state.next_prob_9[child_idx] child_9 = graph[child_key_9] value_uk.append(child_9.g_value) action_idx = np.argmin(value_uk) if explore: action_idx = explore_action(action_idx) child_key = state.next_prob_9[action_idx] trajectory.append(child_key) state = graph[child_key] print('finding feasible path:{},{}'.format(state.px,state.py)) return trajectory
def track_the_best_plan(grid, idx=0): start_node = Node(START_LINE[idx][0], START_LINE[idx][1], 0, 0) start_key = start_node.key state = graph[start_key] trajectory = [state] # for i in range(grid.shape[0]+grid.shape[1]) a safer condition while not state.is_goal: value_uk = [] for child_idx in range(len(ACTION_SPACE)): child_key_9 = state.next_prob_9[child_idx] child_9 = graph[child_key_9] value_uk.append(child_9.g_value) child_key = state.next_prob_9[np.argmin(value_uk)] if np.random.randint(low=0, high=9, size=1)[0] == 8: child_key = state.next_prob_9[np.random.randint(low=0, high=8, size=1)[0]] state = graph[child_key] if grid[state.px, state.py] == START: trajectory.clear() trajectory.append(state) print(state.px, state.py, state.vx, state.vy, state.g_value) return trajectory
def build_up_graph(grid, save_path): max_vel = 5 # velocity dimension vel_list = [] for i_vel in range(-max_vel+1, max_vel): for j_vel in range(-max_vel+1, max_vel): vel_list.append([i_vel, j_vel]) # position dimension x_idx, y_idx = np.where(grid == FREE) coord = np.stack([x_idx, y_idx], axis=1) for p_idx in range(coord.shape[0]): pnt = coord[p_idx] for vel in vel_list: state = Node(pnt[0], pnt[1], vel[0], vel[1]) state.connect_to_graph(grid) graph[state.key] = state for pnt in START_LINE: state = Node(pnt[0], pnt[1], 0, 0) state.connect_to_graph(grid) graph[state.key] = state for pnt in FINISH_LINE: state = Node(pnt[0], pnt[1], 0, 0) state.is_goal = True graph[state.key] = state output = open(save_path, 'wb') pickle.dump(graph, output)
def RTDP(greedy_prob=0.8): itr_num = 0 bellman_error = np.inf bellman_error_list = [] while bellman_error > 0.0001 and itr_num <= 1e4: itr_num += 1 bellman_error = 0.0 # graph is a dict of Node (=state), graph[state.key] = state # state.key is an unique str representing a state (px,py,vx,vy) # trajectory traj = [] # getting the trajectory from value of G rand_start = START_LINE[np.random.randint(low=0, high=3, size=1)[0]] tmp = Node(rand_start[0], rand_start[1], 0, 0) # tmp = Node(0, 3, 0, 0) state = graph[tmp.key] traj.append(state) #print("Trajectory is:",traj) # OPEN = [state] # CLOSED = [] n = 0 NUM_INT = 100 # avoid looping while state.is_goal == False and n <= NUM_INT: # in all reaachable states findoing out the next state n += 1 neighbor_state_key = [] neighbor_state_G = [] #visited_state = [] for child_idx in range(len(ACTION_SPACE)): if ACTION_SPACE[child_idx] == [ 0, 0 ]: # forbidden stay at the same position! continue child_key_9 = state.next_prob_9[ child_idx] # only consider the states that are changed child_9 = graph[child_key_9] if child_9 in traj: continue # forbidden re-visiting node! else: neighbor_state_key.append(child_key_9) neighbor_state_G.append(child_9.g_value) # trick: avoding no state to extend! restart! if neighbor_state_key == []: break # greedy or random. according to greedy_prob if np.random.rand() < greedy_prob: min_idx = np.argmin(neighbor_state_G) next_state_key = neighbor_state_key[min_idx] else: # randomly select an action but not optimal rand_idx = np.random.randint(low=0, high=len(neighbor_state_key)) next_state_key = neighbor_state_key[rand_idx] state = graph[next_state_key] traj.append(state) if n >= NUM_INT: # for st in traj: # print(st.key) print("It seems that there is a loop during greedy policy!") #raise SystemExit(0) # Bacak up the state G value along the trajectory for (state_0, state_1) in zip(traj[0:-1], traj[1:]): # until goal found expected_cost_uk_star = 0.9 * (1 + state_1.g_value) + 0.1 * ( 1 + state_0.g_value) current_value = expected_cost_uk_star bellman_error += np.linalg.norm(state_0.g_value - current_value) state_0.g_value = current_value # update G bellman_error_list.append(bellman_error) print("{}th iteration: {}".format(itr_num, bellman_error)) # end while plt.figure() x_axis = range(len(bellman_error_list)) plt.plot(x_axis, bellman_error_list) plt.show()
super().__init__(node) self.mapping_name = "torch.stack" self.calling_name = "torch.stack" self.arg_keys = ["dim"] def parse_args(self): super(Concatenation, self).parse_args() tensor_arg = "(" + ', '.join([in_node.output_var_name for in_node in self.src_node.input]) + ')' self.args['tensors'] = tensor_arg def to_declare_code(self, out_var_name): return "" # # class ResInception(PytorchBlock): # def __init__(self, node: Block, output_name): # super().__init__(node, output_name) # self.mapping_name = "torch.nn.Conv2D" # self.arg_keys = ["in_channels", "out_channels", "kernel_size", "stride", "padding"] if __name__ == '__main__': node = Node( "pxy is handsome", "Conv2D", "sb", {"in_channels": "1", "out_channels": "2", "kernel_size": "(3,3)", "stride": "1", "padding": "1"} ) conv = Conv2D(node, "out1") print(conv.to_declare_code(""))
def build_up_graph(grid, save_path): max_vel = 5 # velocity dimension vel_list = [] for i_vel in range(-max_vel + 1, max_vel): for j_vel in range(-max_vel + 1, max_vel): vel_list.append([i_vel, j_vel]) # position dimension x_idx, y_idx = np.where(grid == FREE) coord = np.stack([x_idx, y_idx], axis=1) for p_idx in range(coord.shape[0]): pnt = coord[p_idx] for vel in vel_list: state = Node(pnt[0], pnt[1], vel[0], vel[1]) m_dist = np.abs(np.asarray(FINISH_LINE) - np.array([state.px, state.py])) # IMPORTANT-1 Heuristic Function design here # TO BE IMPLEMENTED # Note: Both the two heuristics are not the best # example-1 heuristic = np.linalg.norm(m_dist, axis=1) # Euclidean distance # example-2 heuristic = m_dist[:, 0] + m_dist[:, 1] # Mahalonobis distance state.g_value = np.min(heuristic) state.connect_to_graph(grid) graph[state.key] = state for pnt in START_LINE: state = Node(pnt[0], pnt[1], 0, 0) heuristic = np.linalg.norm(np.asarray(FINISH_LINE) - np.array([state.px, state.py]), axis=1) state.g_value = np.min(heuristic) state.connect_to_graph(grid) graph[state.key] = state for pnt in FINISH_LINE: state = Node(pnt[0], pnt[1], 0, 0) state.is_goal = True graph[state.key] = state output = open(save_path, 'wb') pickle.dump(graph, output)
def build_up_graph(grid, save_path): max_vel = 5 # velocity dimension vel_list = [] for i_vel in range(-max_vel + 1, max_vel): for j_vel in range(-max_vel + 1, max_vel): vel_list.append([i_vel, j_vel]) # 速度的一个范围 ? # position dimension x_idx, y_idx = np.where(grid == FREE) # 记录所有为0的坐标 coord = np.stack([x_idx, y_idx], axis=1) # 每一行都是一个坐标 for p_idx in range(coord.shape[0]): pnt = coord[p_idx] for vel in vel_list: # 每个点都按速度循环????这个有什么意义 state = Node(pnt[0], pnt[1], vel[0], vel[1]) m_dist = np.abs( np.asarray(FINISH_LINE) - np.array([state.px, state.py])) # 前一项的每一个元素都会和后面的相减 # IMPORTANT-1 Heuristic Function design here # TO BE IMPLEMENTED # Note: Both the two heuristics are not the best # example-1 #heuristic = np.linalg.norm(m_dist, axis=1) # Euclidean distance # example-2 #heuristic = m_dist[:, 0] + m_dist[:, 1] # Mahalonobis distance # diagonal heuristic heuristic = m_dist[:, 0] + m_dist[:, 1] + (2**0.5 - 2) * np.min( m_dist, axis=1)[:, np.newaxis] state.g_value = np.min(heuristic) # 取最小的 print(state.g_value) state.connect_to_graph(grid) graph[state.key] = state for pnt in START_LINE: state = Node(pnt[0], pnt[1], 0, 0) heuristic = np.linalg.norm(np.asarray(FINISH_LINE) - np.array([state.px, state.py]), axis=1) state.g_value = np.min(heuristic) state.connect_to_graph(grid) graph[state.key] = state for pnt in FINISH_LINE: state = Node(pnt[0], pnt[1], 0, 0) state.is_goal = True graph[state.key] = state output = open(save_path, 'wb') pickle.dump(graph, output)
from graph_node import Node from vector import Vector from math import atan2 from graph_generator import GraphGenerator from graph import Graph graph_generator = GraphGenerator() graph1 = graph_generator.generate_circular(15, 15, 3 * 15 - 6, 3 * 15 - 6, 50) nx_graph1 = graph1.get_nx_graph() graph_generator.remove_random_edges(3, 7) graph_generator.change_all_nodes_position(2) graph2 = graph_generator.graph nx_graph2 = graph2.get_nx_graph() node1, node2, node3, node4 = Node(0, 10, 10), Node(1, 10, 20), Node(2, 20, 20), Node(3, 20, 10) # graph = Graph([node1, node2, node3, node4], [(0, 1), (1, 2), (2, 3), (3, 0)]) # nx_graph1.add_node(node1.index, pos=(node1.x, node1.y)) # nx_graph1.add_node(node2.index, pos=(node2.x, node2.y)) # nx_graph1.add_node(node3.index, pos=(node3.x, node3.y)) # nx_graph1.add_node(node4.index, pos=(node4.x, node4.y)) # nx_graph1.add_edge(1, 2) # nx_graph1.add_edge(2, 3) # nx_graph1.add_edge(3, 4) # nx_graph1.add_edge(4, 1) print("Nodes of graph: ")
from networkx.algorithms import bipartite import networkx as nx from matplotlib import pyplot as plt from bipartite_graph import BipartiteGraph from graph_edge import Edge from graph_node import Node bipartite_graph = BipartiteGraph([Node(1), Node(2), Node(3)], [Node('A'), Node('B'), Node('C')], [Edge(1, 'A', 5), Edge(1, 'B', 4), Edge(2, 'C', 9), Edge(3, 'C', 2), Edge(2, 'B', 2), Edge(3, 'A', 10)]) nx_graph = bipartite_graph.get_nx_graph() nx_matching_graph = bipartite_graph.get_maximum_weighted_matching(maximum_cardinality=True) # B = nx.Graph() # # Add nodes # B.add_nodes_from(['A','B','C','D','E'], bipartite=0) # B.add_nodes_from([1,2,3,4], bipartite=1) # # Add edges # B.add_edges_from([('A',1),('B',1),('C',1),('C',3),('D',2),('E',3),('E',4)]) # X, Y = ['A','B','C','D','E'], [1, 2, 3, 4] # pos = dict() # pos.update((n, (1, i)) for i, n in enumerate(X)) # put nodes from X at x=1 # pos.update((n, (2, i)) for i, n in enumerate(Y)) # put nodes from Y at x=2 pos1 = nx.get_node_attributes(nx_graph, 'pos') pos2 = nx.get_node_attributes(nx_matching_graph, 'pos') labels = nx.get_edge_attributes(nx_graph, 'weight') plt.figure(1) nx.draw(nx_graph, pos=pos1) nx.draw_networkx_edge_labels(nx_graph, pos1, edge_labels=labels) plt.figure(2)