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 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)
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
# heuristic = np.linalg.norm(m_dist, axis=1)
L1 = m_dist[:, 0] + m_dist[:, 1]
L2 = np.linalg.norm(m_dist, axis=1)
heuristic = 0.5 * L1 + 0.5 * L2
# 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)
