def test_greedy(self):
root_state = [[5, 2, 8], [4, 1, 7], [0, 3, 6]]
goal_state = [[1, 2, 3], [4, 5, 6], [7, 8, 0]]
path1, eval1 = astar.run(root_state, goal_state, astar.evaluate_greedy,
astar.heuristic_misplaced_tiles)
path2, eval2 = astar.run(root_state, goal_state, astar.evaluate_greedy,
astar.heuristic_manhattan_distance)
self.assertNotEqual(len(path1), len(path2))
def test_if_city_block_performs_better(self):
root_state = [[5, 2, 8], [4, 1, 7], [0, 3, 6]]
goal_state = [[1, 2, 3], [4, 5, 6], [7, 8, 0]]
path1, eval1 = astar.run(root_state, goal_state, astar.evaluate_a_star,
astar.heuristic_misplaced_tiles)
path2, eval2 = astar.run(root_state, goal_state, astar.evaluate_a_star,
astar.heuristic_manhattan_distance)
self.assertEqual(path1, path2)
self.assertTrue(eval1.num_of_expanded > eval2.num_of_expanded)
def test_3(self):
root_state = [[2, 3], [1, 0]]
goal_state = [[1, 2], [3, 0]]
path, evaluation = astar.run(root_state, goal_state,
astar.evaluate_a_star,
astar.heuristic_misplaced_tiles)
self.assertEqual(len(path), 5)
root_state = [[3, 2], [1, 0]]
goal_state = [[1, 2], [3, 0]]
path, evaluation = astar.run(root_state, goal_state,
astar.evaluate_a_star,
astar.heuristic_misplaced_tiles)
self.assertEqual(path, None)
def test_8_with_city_block(self):
root_state = [[5, 2, 8], [4, 1, 7], [0, 3, 6]]
goal_state = [[1, 2, 3], [4, 5, 6], [7, 8, 0]]
path, evaluation = astar.run(root_state, goal_state,
astar.evaluate_a_star,
astar.heuristic_manhattan_distance)
self.assertEqual(len(path), 23)
def test_8(self):
root_state = [[5, 2, 8], [4, 1, 7], [0, 3, 6]]
goal_state = [[1, 2, 3], [4, 5, 6], [7, 8, 0]]
path, evaluation = astar.run(root_state, goal_state,
astar.evaluate_a_star,
astar.heuristic_misplaced_tiles)
self.assertEqual(len(path), 23)
def main(filename, display):
dimensions, rowCons, colCons = nn.get_scenarioFromFile(filename)
initialNonogram = nn.get_initialNonogram(dimensions, rowCons, colCons)
if display:
nn.print_nonogram(initialNonogram)
#initialNonogram, stateOk = nn.filterDomains(initialNonogram)
result = astar.run(initialNonogram, display, nn)
if display:
for nonogram in result[0]:
nn.print_nonogram(nonogram)
visualization.run(nonogram, 100)
#print stats
nn.print_resultStats(result)
def runAstar():
global path, waypoints, walls, updatePath, averageP
updatePath = False
curr = np.array([averageP[0], averageP[1]])
selfRot = averageP[3]
rotMatrix = np.array([[np.cos(selfRot), -np.sin(selfRot)],
[np.sin(selfRot), np.cos(selfRot)]])
hinten = rotMatrix.dot(hinten_)
path = astar.run(waypoints, walls, hinten + curr, np.array([4., 5.]))
clearLines("path")
configureLines("path", 5, 0.8, 0.6, 0.2)
for i in range(len(path) - 1):
appendLines("path", path[i][0], path[i][1], 0.45)
appendLines("path", path[i + 1][0], path[i + 1][1], 0.45)
def main(filename, algorithm, display):
scenario = rH.get_scenarioFromFile(filename)
initial_board = rH.get_initialBoard(scenario)
if display:
rH.print_board(initial_board)
if algorithm == 'astar':
result = astar.run(initial_board, display, rH)
elif algorithm == 'bfs':
result = bfs.run(initial_board, display, rH)
elif algorithm == 'dfs':
result = dfs.run(initial_board, display, rH)
if display:
for board in result[0]:
rH.print_board(board)
visualization.run(board, 100)
#print stats
rH.print_resultStats(result)
#pts = numpy.array([[300,300],[300,340],[350,320]],numpy.int32) #用numpy形成坐标列表
#cv2.polylines(img,[pts],True,(0,255,255),2) #画多边形
#窗口等待任意键盘按键输入,0为一直等待,其他数字为毫秒数
cv2.waitKey(0)
while True:
if cv2.waitKey(0) == 13:
print("running...")
#执行路径查找
if p_from[0] == 0 or p_from[1] == 0 or p_to[0] == 0 or p_to[1] == 0:
print("start and end not setting.")
continue
#map_res = astar_0.astar(g_map, p_from, p_to)
astar = astar.Astar(g_map)
map_res = astar.run(p_from, p_to)
print(map_res)
if map_res is None or len(map_res) == 0:
continue
#map_res.reverse()
last_p = map_res[0]
for row, col in map_res[1:]:
#print(row,col)
pre_ro = last_p[0] * H_0
pre_co = last_p[1] * W_0
#img_add_new[pre_ro:pre_ro+H_0, pre_co:pre_co+W_0] = img_add_new_copy[pre_ro:pre_ro+H_0, pre_co:pre_co+W_0]
img_add_new[row * H_0:row * H_0 + H_0,
col * W_0:col * W_0 + W_0] = img_man
cv2.imshow("AStar", img_add_new)
last_p = (row, col)
cv2.waitKey(200)
def startGreedyAlgorithmManhatten(init_state):
return astar.run(init_state, GOAL_STATE, astar.evaluate_greedy,
astar.heuristic_manhattan_distance)
def startGreedyAlgorithmMissplacedTiles(init_state):
return astar.run(init_state, GOAL_STATE, astar.evaluate_greedy,
astar.heuristic_misplaced_tiles)
def startAStarAlgorithmManhatten(init_state):
return astar.run(init_state, GOAL_STATE, astar.evaluate_a_star,
astar.heuristic_manhattan_distance)
robot = env.GetRobots()[0]
# tuck in the PR2's arms for driving
tuckarms(env, robot)
with env:
# the active DOF are translation in X and Y and rotation about the Z axis of the base of the robot.
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
goalconfig = [2.6, -1.3, -pi / 2]
#### YOUR CODE HERE ####
#### Implement the A* algorithm to compute a path for the robot's base starting from the current configuration of the robot and ending at goalconfig. The robot's base DOF have already been set as active. It may be easier to implement this as a function in a separate file and call it here.
goal = np.array([goalconfig[0], goalconfig[1], goalconfig[2]])
start = robot.GetActiveDOFValues()
astar = astar.AStar(env, robot)
astar.run(start, goal)
#### Draw your path in the openrave here (see /usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/examples/tutorial_plotting.py for examples)
#### Draw the X and Y components of the configurations explored by A*
#### Now that you have computed a path, execute it on the robot using the controller. You will need to convert it into an openrave trajectory. You can set any reasonable timing for the configurations in the path. Then, execute the trajectory using robot.GetController().SetPath(mypath);
#### END OF YOUR CODE ###
waitrobot(robot)
raw_input("Press enter to exit...")
def startGreedyAlgorithmManhatten(init_state):
return astar.run(init_state, GOAL_STATE, astar.evaluate_greedy, astar.heuristic_manhattan_distance)
def startGreedyAlgorithmMissplacedTiles(init_state):
return astar.run(init_state, GOAL_STATE, astar.evaluate_greedy, astar.heuristic_misplaced_tiles)
def startAStarAlgorithmManhatten(init_state):
return astar.run(init_state, GOAL_STATE, astar.evaluate_a_star, astar.heuristic_manhattan_distance)
