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)
