Beispiel #1
0
    def __init__(self, name, speed=0.8):
        rospy.init_node(name)
        self.name = name
        self.speed = speed
        self.caught = False
        self.my_pose = GroundtruthPose(name)
        self.cop_poses = [
            GroundtruthPose(c) for c in ['cop_0', 'cop_1', 'cop_2']
        ]
        self.rate_limit = rospy.Rate(100)  # 200ms
        self.res = 0.2
        self.occupancy_grid = OccupancyGrid(self.res, 0.5)
        self.path_finder = AStar('euclidean',
                                 self.occupancy_grid,
                                 resolution=self.res)
        self.map = list(self.path_finder.occupancy_grid.map)
        self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name),
                                                  Twist,
                                                  queue_size=5)
        rospy.Subscriber('/caught', String, self.catch_listener)

        # file for pose history
        self.pose_history = []
        self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name)
        with open(self.pose_history_fn, 'w'):
            pass

        # wait for ground truth poses node to init
        while not self.my_pose.ready or not all(c.ready
                                                for c in self.cop_poses):
            self.rate_limit.sleep()
            continue

        self.target_position, self.path = self.get_random_target_position_and_path(
        )
Beispiel #2
0
def main():

    global window, origin, astar, objective
    window = pygame.display.set_mode(WINDOW_SIZE)
    pygame.display.set_caption("A* Pathfinding")

    gen_rects()

    origin = grid_list[3][5]
    objective = grid_list[12][28]

    astar = AStar(origin, objective)

    gen_objective_distance()
    gen_adjacents()

    loop = True
    start = False

    vel = 30
    while loop:
        window.fill(WHITE)

        #events
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                sys.exit()

            if event.type == pygame.KEYDOWN:
                if event.key == pygame.K_RETURN:
                    start = True
            if event.type == pygame.MOUSEBUTTONUP:
                if pygame.mouse.get_pressed(3)[0]:
                    click_wall(pygame.mouse.get_pos(), True)
                if pygame.mouse.get_pressed(3)[2]:
                    click_wall(pygame.mouse.get_pos(), False)
            if event.type == pygame.MOUSEMOTION:
                if pygame.mouse.get_pressed(3)[0]:
                    click_wall(pygame.mouse.get_pos(), True)
                if pygame.mouse.get_pressed(3)[2]:
                    click_wall(pygame.mouse.get_pos(), False)

        if start:
            vel = 5
            if not astar.found:
                astar.seach()

        pygame.draw.rect(window, RED, objective.rect)
        pygame.draw.rect(window, GREEN, origin.rect)
        draw_grid()

        # if astar.found:
        #     back(vel,astar)
        #     astar.seach_back()
        #     for sqr in astar.path:
        #         pygame.draw.rect(window,BLUE,sqr.rect)

        pygame.display.update()
        fps.tick(vel)
Beispiel #3
0
 def test_map_with_40_nodes_start_5_end_34(self):
     map_40 = Map(Map40.intersections, Map40.roads)
     start_node_id = 5
     dest_node_id = 34
     astar = AStar(map_40)
     self.assertEqual(astar.search(start_node_id, dest_node_id),
                      [5, 16, 37, 12, 34])
Beispiel #4
0
 def test_map_with_40_nodes_start_8_end_24(self):
     map_40 = Map(Map40.intersections, Map40.roads)
     start_node_id = 8
     dest_node_id = 24
     astar = AStar(map_40, start_node_id, dest_node_id)
     self.assertEqual(astar.search(start_node_id, dest_node_id),
                      [8, 14, 16, 37, 12, 17, 10, 24])
    def get_move(self, grid_data, data):
        self.height = data.get("board").get("height")
        self.width = data.get("board").get("width")
        self.head_x = data.get("you").get("body")[0].get("x")
        self.head_y = data.get("you").get("body")[0].get("y")
        self.my_snake_health = data.get("you").get("health")
        self.my_snake_length = len(data.get("you").get("body"))
        self.pathfinder = AStar((self.head_x, self.head_y), grid_data[0], self.width, self.height)
        self.grid_data = grid_data
        self.data = data

        move = self.move_to_food()

        #NOTE FIND TAIL MODE
        if self.my_snake_length > 3 and self.my_snake_health > 65 or move == None:
            move = self.chase_tail(self.my_snake_health == 100)

        if move:
            return move
        else:
            backup_moves = self.helper.get_backup_move((self.head_x, self.head_y), self.grid_data[0], self.height, self.width)
            if backup_moves:
                return self.helper.get_move_letter((self.head_x, self.head_y), backup_moves[0])
            else:
                return 'up'
    def get_move(self, grid_data, data):
        self.height = data.get("board").get("height")
        self.width = data.get("board").get("width")
        self.head_x = data.get("you").get("body")[0].get("x")
        self.head_y = data.get("you").get("body")[0].get("y")
        self.my_snake_health = data.get("you").get("health")
        self.my_snake_length = len(data.get("you").get("body"))
        self.pathfinder = AStar((self.head_x, self.head_y), grid_data[0], self.width, self.height)
        self.grid_data = grid_data
        self.data = data

        snakes = data.get("board").get("snakes")
        head = (self.head_x, self.head_y)

        # find closest snake ID
        target_snake_id = self.find_closest_snake_head(snakes)

        # Guard to make sure our snake doesn't just starve
        if self.my_snake_health < 40:
            return self.default_behaviour()

        if target_snake_id:
            return self.attack_snake_head(snakes, target_snake_id)
        else:
            return self.default_behaviour()
Beispiel #7
0
 def __init__(self):
   self._cursor = (1, 1)
   self._start = (1, 1)
   self._goal = (33, 8)
   self._console = Console()
   self._map = Map()
   self._astar = AStar(self._map.cost)
Beispiel #8
0
    def compute_route(self, node_source, source_ori, node_target, target_ori):

        self._previous_node = node_source

        a_star = AStar()
        a_star.init_grid(
            self._map.get_graph_resolution()[0],
            self._map.get_graph_resolution()[1],
            self._map.get_walls_directed(node_source, source_ori, node_target,
                                         target_ori), node_source, node_target)

        route = a_star.solve()

        # JuSt a Corner Case
        # Clean this to avoid having to use this function
        if route is None:
            a_star = AStar()
            a_star.init_grid(
                self._map.get_graph_resolution()[0],
                self._map.get_graph_resolution()[1],
                self._map.get_walls_directed(node_source,
                                             source_ori,
                                             node_target,
                                             target_ori,
                                             both_walls=False), node_source,
                node_target)

            route = a_star.solve()

        self._route = route

        return route
Beispiel #9
0
def move():
    data = bottle.request.json
    board = data.get('board')
    board_height = board.get('height')
    board_width = board.get('width')

    # print(board)
    GRID = buildGrid(board, board_height, board_width)
    # print(GRID)
    # TODO: Do things with data
    # get the snake
    VINCE = data['you']
    xhead = VINCE['body'][0]['x']
    yhead = VINCE['body'][0]['y']
    head_tuple = (xhead, yhead)

    # first food bit
    first_food_bit = (board['food'][0]['x'], board['food'][0]['y'])
    AS = AStar()
    path_to_food = AS.find_path(GRID, head_tuple, first_food_bit)
    print(path_to_food)
    directions = ['up', 'down', 'left', 'right']
    direction = random.choice(directions)
    direction = get_dir(head_tuple, path_to_food[1])
    print(direction)
    return {'move': direction, 'taunt': 'battlesnake-python!'}
    def create_astar(self):
        self.bfs = AStar(self.bfs_grid, 'BFS')
        self.dfs = AStar(self.dfs_grid, 'DFS')
        self.astar = AStar(self.astar_grid, 'AStar')

        self.active_search = self.bfs
        self.active_grid = self.bfs_grid
Beispiel #11
0
 def __init__(self):
     self.astar=AStar(MAP_COLS,MAP_ROWS)
     self.map=self.astar.map
     self.astar.print_map()
     self.game_over=False
     self.score=0
     self.done=False
Beispiel #12
0
	def create(self, method_name):
		if method_name == "dijkstra":
			return Dijkstra()
		elif method_name == "astar":
			return AStar()
		else:
			return AStar()
Beispiel #13
0
def main():
    parser = argparse.ArgumentParser(
        description="Compute the optimal path using A*"
    )
    group = parser.add_mutually_exclusive_group()
    group.add_argument(
        "-w", "--world", help="The filename of the world to load",
    )
    group.add_argument(
        "-s", "--size", type=int, help="Size of the random board to be generated"
    )
    parser.add_argument(
        "-f", "--file", help="The file to output the world to"
    )
    parser.add_argument(
        "heuristic", type=int,
        help="The heuristic number to run (in range(1, 7))"
    )
    args = parser.parse_args()
    if args.world:
        newWorld = World(filepath=args.world)
    elif args.size:
        newWorld = World(height=args.size, width=args.size)
    else:
        newWorld = World(height=10, width=10)
    if args.file:
        newWorld.write_world(args.file)
    astar = AStar(newWorld, args.heuristic)
    astar.start()
Beispiel #14
0
class AStarApplication(QWidget):
    def __init__(self):
        super(AStarApplication, self).__init__()
        self.close = self.cleanup

        self.layout = QVBoxLayout(self)
        self.setLayout(self.layout)

        self.canvas = QGraphicsView()
        self.canvas.setViewportUpdateMode(QGraphicsView.FullViewportUpdate)

        self.grid = Grid(24, 20)
        self.tiles = []
        for x in range(self.grid.w):
            for y in range(self.grid.h):
                tile = Tile(self.grid.get_node(x, y), self)
                self.tiles.append(tile)

        self.scene = QGraphicsScene()
        self.scene.setBackgroundBrush(QBrush(Qt.lightGray, Qt.SolidPattern))

        for t in self.tiles:
            self.scene.addItem(t)

        self.canvas.setScene(self.scene)
        self.layout.addWidget(self.canvas)

        self.refresh_timer = QTimer()
        self.refresh_timer.setInterval(1000 / 60)
        self.refresh_timer.timeout.connect(self.refresh)
        self.refresh_timer.start()

        self.AStar = AStar(self.grid)
        self.focusPolicy = Qt.StrongFocus
        self.keyPressEvent = self.kbdHandler

    # Avoid the segfault caused by some obscure GC stuff.
    @pyqtSlot()
    def cleanup(self, obj):
        pass

    @pyqtSlot()
    def refresh(self):
        self.scene.invalidate()

    @pyqtSlot()
    def kbdHandler(self, e):
        if e.key() == Qt.Key_Space:
            self.AStar.step()

    # Called when a tile changes type, so all tiles can be updated
    def update_tiles(self, caller):
        for t in self.tiles:
            if self.grid.target is not None:
                t.subtext = t.node.heuristic(self.grid.target)
            else:
                t.subtext = ''
            if t != caller and t.type != 'clear' and t.type != 'wall' and t.type == caller.type:
                t.set_type('clear')
        self.AStar.reset()
Beispiel #15
0
    def __init__(self):
        super(AStarApplication, self).__init__()
        self.close = self.cleanup

        self.layout = QVBoxLayout(self)
        self.setLayout(self.layout)

        self.canvas = QGraphicsView()
        self.canvas.setViewportUpdateMode(QGraphicsView.FullViewportUpdate)

        self.grid = Grid(24, 20)
        self.tiles = []
        for x in range(self.grid.w):
            for y in range(self.grid.h):
                tile = Tile(self.grid.get_node(x, y), self)
                self.tiles.append(tile)

        self.scene = QGraphicsScene()
        self.scene.setBackgroundBrush(QBrush(Qt.lightGray, Qt.SolidPattern))

        for t in self.tiles:
            self.scene.addItem(t)

        self.canvas.setScene(self.scene)
        self.layout.addWidget(self.canvas)

        self.refresh_timer = QTimer()
        self.refresh_timer.setInterval(1000 / 60)
        self.refresh_timer.timeout.connect(self.refresh)
        self.refresh_timer.start()

        self.AStar = AStar(self.grid)
        self.focusPolicy = Qt.StrongFocus
        self.keyPressEvent = self.kbdHandler
Beispiel #16
0
    def send_pose_sp(self):

        if self.occupancy and self.has_robot_location and self.nav_sp:
            state_min = (-int(round(self.plan_horizon)),
                         -int(round(self.plan_horizon)))
            state_max = (int(round(self.plan_horizon)),
                         int(round(self.plan_horizon)))
            # Round initial, goal positions to grid resolution
            x_init = round_pt_to_grid(self.robot_translation[:2],
                                      self.plan_resolution)
            x_goal = round_pt_to_grid(self.nav_sp[:2], self.plan_resolution)

            astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
                          self.plan_resolution)

            # uncomment to add buffering to obstacles
            bufferRadius = 4
            astar.bufferOccupancy(bufferRadius)

            rospy.loginfo("Computing new navigation plan")
            if astar.solve():
                # If initial state == goal, path len == 1
                # Handle case where A* solves, but no 2nd element -> don't send msg
                if len(astar.path) < self.short_path_len:
                    rospy.loginfo("Path goal matches current state")

                # Typical use case
                else:
                    wp_x, wp_y, wp_th = self.next_waypoint(astar)
                    self.publish_path(astar, wp_x, wp_y, wp_th)

            else:
                rospy.logwarn("Could not find path")
Beispiel #17
0
    def __init__(self, name, speed=0.6):
        rospy.init_node(name)
        self.name = name
        self.speed = speed
        self.my_pose = GroundtruthPose(name)
        self.occupancy_grid = OccupancyGrid(0.2, 0.5)
        # assign cop to unique robber
        self.target = 'robber_' + name[-1]
        self.target_pose = GroundtruthPose(self.target)
        self.rate_limit = rospy.Rate(100)  # 200ms
        self.path_finder = AStar('euclidean',
                                 self.occupancy_grid,
                                 resolution=0.2)
        self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name),
                                                  Twist,
                                                  queue_size=5)
        self.catch_publisher = rospy.Publisher('/caught', String, queue_size=1)
        rospy.Subscriber('/caught', String, self.catch_listener)

        # file for pose history
        self.pose_history = []
        self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name)
        with open(self.pose_history_fn, 'w'):
            pass

        # wait for ground truth poses node to init
        while not self.my_pose.ready and not self.target_pose.ready:
            self.rate_limit.sleep()
            continue
Beispiel #18
0
    def __init__(self):
        self.list = []
        count = 0
        #  依次添加行人至self.list
        for i in range(0, 15):
            self.list.append(People("o" + str(count), 60, 60 + i * 40))
            count = count + 1
            self.list.append(People("o" + str(count), 100, 60 + i * 40))
            count = count + 1
            self.list.append(People("o" + str(count), 140, 60 + i * 40))
            count = count + 1

        # 在一开始就计算好各个位置的下一步方向向量,并存储到矩阵中,以节省算力
        k = 5
        self.matrix = [[0 for i in range(17)] for i in range(27)]
        for i in range(0, 27):
            for j in range(0, 17):
                if i == 0:  # 将障碍物的位置也设置对应的方向向量,以便在特殊情况撞墙后能及时调整回来
                    self.matrix[i][j] = (k, 0)
                elif i == 26:
                    self.matrix[i][j] = (-1 * k, 0)
                elif j == 0:
                    self.matrix[i][j] = (0, k)
                elif j == 16:
                    self.matrix[i][j] = (0, -1 * k)
                elif i == 10:
                    if (j == 4) or (j == 10):
                        self.matrix[i][j] = (-1 * k, -1 * k)
                    elif (j == 5) or (j == 11):
                        self.matrix[i][j] = (-1 * k, 0)
                    elif (j == 6) or (j == 12):
                        self.matrix[i][j] = (-1 * k, k)
                    else:
                        self.matrix[i][j] = AStar.next_loc(i, j)
                elif i == 17:
                    if (j == 4) or (j == 10):
                        self.matrix[i][j] = (k, -1 * k)
                    elif (j == 5) or (j == 11):
                        self.matrix[i][j] = (k, 0)
                    elif (j == 6) or (j == 12):
                        self.matrix[i][j] = (k, k)
                    else:
                        self.matrix[i][j] = AStar.next_loc(i, j)
                elif (j == 4) or (j == 10):
                    if (i > 10) and (i < 17):
                        self.matrix[i][j] = (0, -1 * k)
                    else:
                        self.matrix[i][j] = AStar.next_loc(i, j)
                elif (j == 6) or (j == 12):
                    if (i > 10) and (i < 17):
                        self.matrix[i][j] = (0, k)
                    else:
                        self.matrix[i][j] = AStar.next_loc(i, j)
                elif (i > 10) and (i < 17) and ((j == 5) or (j == 11)):
                    self.matrix[i][j] = (0, k)
                else:
                    self.matrix[i][j] = AStar.next_loc(i, j)
        self.matrix[26][4] = (1, 0)
Beispiel #19
0
 def calculatePath(self, start, goal, clusterIds):
     # Only length
     starSolver = AStar(self)
     t = starSolver.solveBetweenNodes(clusterIds, start, goal)
     if t > 0:
         self.graph.addEdge(start, goal, t)
         return True
     else:
         return False
Beispiel #20
0
 def calculatePath(self, start, goal, clusterIds):
     # Only length
     starSolver = AStar(self)
     t = starSolver.solveBetweenNodes(clusterIds, start, goal)
     if t > 0:
         self.graph.addEdge(start, goal, t)
         return True
     else:
         return False
Beispiel #21
0
def main(argv):
    # queens = createBoard(argv[0])
    queens = getDemoBoard(argv[0])
    heur = 0 if argv[2] == "H1" else 1

    if int(argv[1]) == 1:   # A* Algorithm
        astar_solver = AStar(queens, heur)
        print(astar_solver.Solve())
    elif int(argv[1]) == 2:  # Hill Climb
        hill_solver = HillClimb(queens, heur)
        hill_solver.solve()
Beispiel #22
0
def startGame():
    mystdout = StdOutWrapper()
    sys.stdout = mystdout
    sys.stderr = mystdout

    curses.initscr()
    curses.beep()
    curses.beep()
    window = curses.newwin(HEIGHT, WIDTH, 0, 0)
    window.timeout(TIMEOUT)
    window.keypad(1)
    curses.noecho()
    curses.curs_set(0)
    window.border(0)

    snake = Snake(SNAKE_X, SNAKE_Y, window)
    food = Food(window, snake)
    astar = AStar()

    while True:
        window.clear()
        window.border(0)

        # rendering the objects
        snake.render()
        food.render()

        window.addstr(0, 5, snake.getScore)
        event = window.getch()

        if event == 27:
            break

        if snake.head.x == food.x and snake.head.y == food.y:
            snake.eatFood(food)
            curses.beep()

        event = astar.getKey(food, snake)
        # print(event)

        if event in (KEY_UP, KEY_DOWN, KEY_LEFT, KEY_RIGHT):
            snake.makeMove(event)

        snake.update()
        if snake.collided():
            break

    curses.endwin()
    print(f"High score :: {snake.score}")

    sys.stdout = sys.__stdout__
    sys.stderr = sys.__stderr__
    sys.stdout.write(mystdout.get_text())
Beispiel #23
0
 def __call__(self, matrix, snake, candy_pos):
     if self.path:
         return self.get_direction(snake[0], self.path.pop(0))
     head2candy, _ = AStar(matrix, snake[0], candy_pos, walls="qzpmublrto")
     if head2candy:
         sim_matrix, sim_tail = self.simulate(matrix, snake, head2candy)
         candy2tail, _ = AStar(sim_matrix, candy_pos, sim_tail, walls="qzpmublro")
         if candy2tail:
             self.path = head2candy[2:]
             return self.get_direction(snake[0], head2candy[1])
     head2tail, _ = AStar(matrix, snake[0], snake[-1], walls="qzpmublro")
     return self.get_direction(snake[0], head2tail[1])
Beispiel #24
0
    def find_alternative_path(self, start, blackout, goal):
        if start == goal: return True
        if (start, blackout, goal) in self.alternative_paths: return True

        backup_val = self.grid[blackout]
        self.grid[blackout] = inf
        pather = AStar(self.grid, start, goal)
        success = pather.findpath()
        if success:
            self.alternative_paths[(start, blackout, goal)] = pather.path
            self.alternative_paths[(goal, blackout, start)] = deque(reversed(pather.path))
        self.grid[blackout] = backup_val
        return success
def find_closest(x_init, x_goals, statespace_lo, statespace_hi, occupancy,
                 resolution):
    min_dist = np.float('inf')
    closest = None
    for x_goal in x_goals:
        astar = AStar(statespace_lo, statespace_hi, x_init, x_goal, occupancy,
                      resolution)

        if astar.solve() and len(astar.path) < min_dist:
            min_dist = len(astar.path)
            closest = x_goal

    return closest, min_dist
Beispiel #26
0
    def send_pose_sp(self):

        try:
            (robot_translation,
             robot_rotation) = self.trans_listener.lookupTransform(
                 "/map", "/base_footprint", rospy.Time(0))
            self.has_robot_location = True
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException):
            robot_translation = (0, 0, 0)
            robot_rotation = (0, 0, 0, 1)
            self.has_robot_location = False

        if self.occupancy and self.has_robot_location and self.nav_sp:
            state_min = (-int(round(self.plan_horizon)),
                         -int(round(self.plan_horizon)))
            state_max = (int(round(self.plan_horizon)),
                         int(round(self.plan_horizon)))
            x_init = (round(robot_translation[0] / self.plan_resolution) *
                      self.plan_resolution,
                      round(robot_translation[1] / self.plan_resolution) *
                      self.plan_resolution)
            x_goal = ((round(self.nav_sp[0] / self.plan_resolution)) *
                      self.plan_resolution,
                      round(self.nav_sp[1] / self.plan_resolution) *
                      self.plan_resolution)
            rospy.logwarn("x_init (after rounding): (%6.4f,%6.4f)", x_init[0],
                          x_init[1])
            rospy.logwarn("x_goal (after rounding): (%6.4f,%6.4f)", x_goal[0],
                          x_goal[1])
            astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
                          self.plan_resolution)

            rospy.loginfo("Computing Navigation Plan")

            if astar.solve():
                rospy.loginfo(
                    "Navigation Success. Found %d waypoint path to (%6.3f, %6.3f)",
                    len(astar.path), astar.path[-1][0], astar.path[-1][1])
                # a naive path follower we could use
                # pose_sp = (astar.path[1][0],astar.path[1][1],self.nav_sp[2])
                # msg = Float32MultiArray()
                # msg.data = pose_sp
                # self.pose_sp_pub.publish(msg)
                # astar.plot_path()

                path_msg = self.buildPath(astar.path)
                self.nav_path_pub.publish(path_msg)

            else:
                rospy.logwarn("Navigation Failure")
async def step_4(robot1, robot2, currentPos1, currentPos2, destPos):
    # Navigation procedure as in Step 2.

    # ROBOT 1
    a = AStar(robot1, currentPos1)

    print('Navigating from:', (4, 6), 'to', (8, 4))
    print('')

    path = a.initLegoWorld((4, 6), (8, 4))
    for i in range(len(path)):
        await a.move(path[i])

    await a.face("north")
    currentPos1 = a.getPos()

    # ROBOT 2
    a = AStar(robot2, currentPos2)

    print('Navigating from:', (2, 6), 'to', (6, 4))
    print('')

    dest = (destPos[0], destPos[1])

    path = a.initLegoWorld((2, 6), dest)
    for i in range(len(path)):
        await a.move(path[i])

    await a.face("east")
    currentPos1 = a.getPos()

    return True, currentPos1, currentPos2
Beispiel #28
0
def run():
    # Initialize a 10x10 stage. No other stage sizes are
    # coded yet, but these constructor arguments exist in
    # case we want to later on
    board = Board()

    # Initialize the A-Star algorithm class with our
    # current graph and reference to the board because
    # I'm lazy. It shouldn't have a dependency on a board,
    # but it's been 8 hours already and I'm tired.
    astar = AStar(board.graph, board)
    # Render the board to show where we stand
    board.render()
    inp = get_input()
    while inp[0].lower() not in "qe":
        if inp[0].lower() in "h":
            # Players is babies and wants helps!
            with open(os.path.join(resource_root, "maze", "help.txt"), "rb") as help_file:
                print(help_file.read())
            board.render()
            inp = raw_input("Next move? (enter h or help for help)").strip()
            if inp == "":
                inp = " "
            continue
        # Try to move the player. Run, Forrest! Run!
        if inp[0].lower() == "u":
            (x, y) = board.player
            board.move_entity("player", x, y - 1)
        elif inp[0].lower() == "d":
            (x, y) = board.player
            board.move_entity("player", x, y + 1)
        elif inp[0].lower() == "l":
            (x, y) = board.player
            board.move_entity("player", x - 1, y)
        elif inp[0].lower() == "r":
            (x, y) = board.player
            board.move_entity("player", x + 1, y)

        # The enemy is a greedy bastard and moves every chance he can
        path = astar.get_path(astar.search(board.enemy, board.player)[0], board.enemy, board.player)
        board.move_entity("enemy", *path[1])
        board.render(path=path[2:-1])
        if board.enemy == board.player:
            print("You lost!")
            exit()
        if board.player == board.goal:
            print("You escaped death! Huzzah!")
            exit()
        inp = get_input()
    print("Scaredy-cat!")
async def step_6(robot1, robot2, currentPos1, currentPos2):
    # Navigation procedure as in Step 2.

    # ROBOT 2
    a = AStar(robot2, currentPos2)

    print('Navigating from:', (6, 4), 'to', (4, 0))
    print('')

    path = a.initLegoWorld((6, 4), (4, 0))
    for i in range(len(path)):
        await a.move(path[i])

    await a.face("south")
    currentPos2 = a.getPos()

    # ROBOT 1
    a = AStar(robot1, currentPos1)

    print('Navigating from:', (8, 4), 'to', (4, 2))
    print('')

    path = a.initLegoWorld((8, 4), (4, 2))
    for i in range(len(path)):
        await a.move(path[i])

    currentPos1 = a.getPos()

    await robot1.drive_straight(distance_mm(150),
                                speed_mmps(50)).wait_for_completed()
    await robot1.turn_in_place(degrees(70)).wait_for_completed()

    return True, currentPos1, currentPos2
async def step_8(robot1, robot2, currentPos1, currentPos2):
    # Undo the movements made by the robots when trying to 'look natural' in Step 6.
    await robot1.turn_in_place(degrees(-70)).wait_for_completed()
    await robot1.drive_straight(distance_mm(-150),
                                speed_mmps(50)).wait_for_completed()

    # Navigation procedure as in Step 2.

    # ROBOT 1
    a = AStar(robot1, currentPos1)

    print('Navigating from:', (4, 2), 'to', (0, 2))
    print('')

    path = a.initLegoWorld((4, 2), (0, 2))
    for i in range(len(path)):
        await a.move(path[i])

    currentPos1 = a.getPos()

    # ROBOT 2
    a = AStar(robot2, currentPos2)

    print('Navigating from:', (4, 0), 'to', (0, 6))
    print('')

    path = a.initLegoWorld((4, 0), (0, 6))
    for i in range(len(path)):
        await a.move(path[i])

    await a.face("south")
    currentPos2 = a.getPos()

    return True, currentPos1, currentPos2
Beispiel #31
0
 def main(self):
     clock = pygame.time.Clock()
     graph = grid2graph(self.world)  # Convert grid into graph
     astar = AStar()  # Initialise Astar
     res = 25  # Resolution = cm per grid square
     while not self.quit:
         source = self.node_nb(self.buggy_pos, res)  # Source = buggy pos
         sink = self.node_nb(self.cur_pos,
                             res * self.scale)  # Sink = cursor pos
         [node.reset() for node in graph]  # Reset every node
         path = astar.solve(graph, source, sink)  # Solve graph
         self.event_handler(
         )  # Put all keypress events into key_status dictionary
         self.update_window(path, res)  # Update display
         clock.tick(self.fps)  # Restrict rate
Beispiel #32
0
    def callAstar(self):
        occupancy = DetOccupancyGrid2D(self.map_width, self.map_height,
                                       self.obstacles)
        self.astar = AStar((0, 0), (self.map_width, self.map_height),
                           self.start_pos, self.end_pos, occupancy)

        if not self.astar.solve():
            exit(0)

        self.trajectory = Trajectory()
        self.trajectory.x.data = (self.astar.path * self.astar.resolution)[:,
                                                                           0]
        self.trajectory.y.data = (self.astar.path * self.astar.resolution)[:,
                                                                           1]
        self.trajectory.theta.data = np.zeros((len(self.trajectory.x.data), 1))
Beispiel #33
0
    def find_solver(self, algorithm):
        solver = None
        while solver is None:
            if algorithm == 'depth_first' or algorithm == 'dfs':
                solver = Depth_first(self.map)
            elif algorithm == 'breadth_first' or algorithm == 'bf' or algorithm == 'bfs':
                solver = Breadth_first(self.map)
            elif algorithm == 'astar' or algorithm == 'a*':
                solver = AStar(self.map)
            elif algorithm == 'greedy_best_first' or algorithm == 'gbf' or algorithm == 'gbfs':
                solver = Greedy_best_first(self.map)
            elif algorithm == 'iterative_deepening' or algorithm == 'id' or algorithm == 'ids':
                solver = Iterative_deepening(self.map)
            elif algorithm == 'greedy' or algorithm == 'g' or algorithm == 'gs':
                solver = Greedy(self.map)

            #Iterative astar works, but it does not work well.
            #I did not research it much before building it, and I got a few key details wrong.
            #I decided to scrap it and use a different informed custom search (greedy), as it would have taken me far too long to redo it.
            #But I figured it would still be a valuable thing to include just to show my progress.
            elif algorithm == 'iterative_astar' or algorithm == 'ia*':
                solver = Iterative_astar(self.map)
            else:
                print(
                    'Valid searches are: dfs (depth first), bfs (breadth first), A*, greedy, ids (iterative deepening) or gbfs (greedy best first)'
                )
                print('Please enter an algorithm:')
                algorithm = input()

        return solver
Beispiel #34
0
    def update(cls, screen, survivor):
        del_zmbs = set()
        for zmb in cls.instances:
            if zmb.health <= 0.:
                Drop.spawn(zmb.pos)
                del_zmbs.add(zmb)
                stats["Zombies Killed"] += 1
                continue

            screen.blit(zmb.img, zmb.pos)
            zmb.health_bar(surface=screen)  # Health bar with rounded edges
            if Options.debug:
                for tile in zmb.path:
                    pygame.draw.circle(screen, zmb.path_colour,
                                       tile.get_centre(), Tile.length // 3)

            zmb_to_survivor_dist = (survivor.pos - zmb.pos).magnitude()

            if zmb_to_survivor_dist <= cls.attack_range:
                survivor.health -= 0.4

            if zmb.to is not None:  # if the zombie is not next to the player
                angle = angle_between(*zmb.pos, *(zmb.to + zmb.vel))
                if zmb.pos == zmb.to:  # If the zombie is directly on a tile
                    zmb.to = None  # Trigger A-Star, not run between tiles for performance
                else:
                    if "freeze" not in Drop.actives:
                        zmb.pos += zmb.vel
                if zmb.direction != angle:  # New direction, frame after a turn
                    zmb.rotate(angle)

            if zmb.to is None and zmb_to_survivor_dist > Tile.length:
                AStar(zmb, survivor).solve()
        cls.instances -= del_zmbs
Beispiel #35
0
    def test_heuristic_gen(self):
        root = self.gen_tree()
        pvar = pred_variance(root)
        max_var = 1.5 * max(pvar.values())
        pvar1 = dict((p, max_var - v) for p, v in pvar.items())
        pvar1s = sum(pvar1.values())
        weights = dict((p, root.num_occurrences(p) * v / pvar1s)
                       for p, v in pvar1.items())
        for p, w in weights.items():
            print '%s: %f' % (p, w)

        preds = list(root.get_all_predicates())
        preds.sort()
        weights = fmincon.run_fmincon(root)

        h = heuristic.Heuristic(dict(zip(preds, weights)))

        root.make_graphviz(open('temp.dot', 'w'))
        search = AStar(root, lambda x: h(x.preds))
        iter = search.gen
        iterations = 0
        try:
            while not iter.next():
                iterations += 1
        except StopIteration:
            print iterations
            print[''.join(s.preds) for s in search.solution]
class AStarGAC:

    def __init__(self):
        # Keep track of all the nodes in the system
        self.nodes = []

        # Set node type for AStar
        AStar.State = GACState

        # Reference to AStar and CSP
        self.astar = AStar()
        self.gac_state = None

        # Set behavior
        self.astar.set_behavior(BestFirst)

    def start(self):
        # Run CSP-initialize and CSP-domain-filtering-loop
        self.gac_state.gac.initialize()
        self.gac_state.gac.domain_filtering()

        # Assign the current state to AStar and begin to solve
        self.astar.states[self.gac_state.get_hash()] = self.gac_state
        self.astar.behavior.add(self.gac_state, self.astar.open)

        # Check if we are finished or if the current state is invalid
        if self.gac_state.gac.check_finished() or self.gac_state.gac.check_contradictory_state():
            if self.gac_state.gac.check_finished():
                # We found the solution without actually running the agenda loop, make sure we save goal state
                self.gac_state.type = self.astar.State.GOAL
                return True
            return False

    def run(self):
        if self.gac_state.gac.check_finished():
            return True

        # Do one step in the agenda loop
        finished = self.astar.agenda_loop()

        # Update the CSP state
        self.gac_state = self.astar.closed[-1]

        # Return the result from the agenda loop
        return finished
    def __init__(self):
        # Keep track of all the nodes in the system
        self.nodes = []

        # Set node type for AStar
        AStar.State = GACState

        # Reference to AStar and CSP
        self.astar = AStar()
        self.gac_state = None

        # Set behavior
        self.astar.set_behavior(BestFirst)
Beispiel #38
0
    def step(self, execute):
        if self.mmap.isLiftOpen():
            self.tar = self.mmap.getLift()
            aStar = AStar(self.mmap.copy(), self.tar)
            self.path = aStar.process().path()
        else:
            self.tars = []
            for l in self.mmap.getLambdas():
                aStar = AStar(self.mmap.copy(), l)
                self.path = aStar.process().path()
                self.tars.append((l, self.path))
            minmin = reduce(lambda a, b: min(a, b), map(lambda t: len(t[1]), self.tars))
            self.tar, self.path = filter(lambda t: len(t[1]) == minmin, self.tars)[0]

        if (not self.wouldGetKilledFor(self.path) and 
            self.mmap.getLeftCommands() > len(self.path) and
            len(self.path) > 0):
            execute(self.path)
            self.skip = []
        else:
            self.skip.append(self.tar)

        return self
Beispiel #39
0
    def move(self):

        if self.next_step_x == None and self.next_step_y == None:

            if self.player.x != self.player_last_position[0] or self.player.y != self.player_last_position[1]:
                
                self.player_last_position = (self.player.x, self.player.y)

                if self.player.next_step_x == None and self.player.next_step_y == None:
                    self.path = AStar.get_path(self)


        if self.next_step_x == None and  self.next_step_y == None:

            if len(self.path) > 0:
                l = self.path.pop()
                self.next_step_x = l.x
                self.next_step_y = l.y

        else:

            if self.x > self.next_step_x:
                self.x -= 4
                self.direction = "W"
            elif self.x < self.next_step_x:
                self.x += 4
                self.direction = "E"

            if self.y > self.next_step_y:
                self.y -= 4
                self.direction = "N"
            elif self.y < self.next_step_y:
                self.y += 4
                self.direction = "S"

            if self.x == self.next_step_x and self.y == self.next_step_y:
                self.next_step_x = None
                self.next_step_y = None
class Astar_GAC(Graph): 
    """Astar_GAC integrates Astar and GAC"""
    #both
    def __init__(self, variables, domains, expressions):
        super(Astar_GAC, self).__init__()
        self.cnet = CNET(variables, domains, expressions)
        self.currentState = self.initializeState(self.cnet)
        self.gac = GAC(self.currentState)
        self.Astar = AStar(self)


    @abstractmethod
    def createNewState( self, variables, constraints):
        pass 


    def initializeState(self, cnet):
        """in initState each variable has its full domain. It will be set as root node
        initilizes cnet"""
        s = self.createNewState( cnet.variables, cnet.constraints )
        s.update('start')
        self.startNode = s
        self.stateCounter = 0
        self.nofAssumption = 0
        self.nofExpanded = 0
        return s 


    def search(self):
        self.currentState = self.gac.domainFiltering(self.currentState)
        self.stateCounter += 1
        
        if self.currentState.isSolution():
            self.printStatistics(self.currentState)
            return self.currentState

        return self.iterateSearch()


    def iterateSearch(self):
            prev = self.currentState 
            if prev.isSolution():
                return prev

            self.currentState = self.Astar.iterateAStar()
            # self.currentState.updateColors()
            self.stateCounter += 1
            self.currentState.parent = prev #used for backtracking to find 'shortest path' for statistics
            self.nofExpanded = self.Astar.nofExpandedNodes
            if self.currentState.isSolution():
                self.printStatistics(self.currentState)
                return self.currentState

            self.currentState = self.gac.domainFiltering(self.currentState)
            return self.currentState


    def makeAssumption(self, newVI, parentState):
        """Generate one successor, and make sure all pointers are correct"""

        newVertices = {}
        newVIList = []
        for vi in parentState.viList:
            viID = vi.getID()
            tmpVI = VI(viID, vi.domain.copy())
            newVertices[viID] = tmpVI
            newVIList.append( tmpVI )

        for vi in parentState.viList:
            viID = vi.getID()
            for neighbor in vi.neighbors:
                n = newVertices[ neighbor.getID() ]
                newVertices[viID].add_neighbor( n )

        for vi in newVIList:
            if vi.getID() == newVI.getID():
                newVIList.remove(vi)
                newVIList.append(newVI)
            else:
                for vi_n in vi.neighbors:
                    if vi_n.getID() == newVI.getID():
                        vi.neighbors.remove(vi_n)
                        vi.neighbors.append(newVI)

        succ = self.createNewState(newVIList, parentState.constraintList)
        succ.parent = parentState
        succ.updateUndecided() # maybe not needed

        succ.ciList = []
        constraints = self.cnet.getConstraints()
        for v in succ.undecidedVariables:
            for n in v.neighbors:
                for c in constraints:
                    succ.ciList.append( CI(c,[v,n]) )

        return succ


    def generateSucc(self, state):
        """ make a guess. start gussing value for variables with min. domain length"""
        succStates = []
        finishedVIs = []
        varsCopy = state.undecidedVariables.copy()

        if not len(varsCopy):
            return []

        otherVIs = sorted(varsCopy, key=lambda v: len(v.domain), reverse=True)
        betterVI = otherVIs.pop()

        if betterVI.domain:
            initID = betterVI.getID()
            for d in betterVI.domain:
                newVI = VI( initID, [d])
                newVI.neighbors = betterVI.neighbors.copy()
                successor = self.makeAssumption(newVI, state)

                succStates.append( self.gac.rerun(successor) )
            return succStates
        else:
            return []


# Not complete : TODO
    def printStatistics(self, state):
        print ( 'The number of unsatisfied constraints = ', self.countUnsatisfiedConstraints(state), '\n' )
        print ( 'The total number of verticies without color assignment = ', self.countColorLess(state), '\n' )
        print ( 'The total number of nodes in search tree = ', self.stateCounter, '\n' )
        print ( 'The total number of nodes poped from agenda and expanded = ', self.nofExpanded, '\n' )
        print ( 'The length of the path = ', self.nofAssumption ,'\n')


    def countColorLess(self, state):
        nofColorLess = 0
        for vi in state.viList:
            if len(vi.domain) != 1:
                nofColorLess += 1
        return nofColorLess


    # def countUnsatisfiedConstraints(self, state):
    #     unsatisfied = 0
    #     varList = state.viList
    #     for c in state.ciList:
    #         for var in varList: 
    #             if var in c.variables:
    #                 if self.countInconsistentDomainValues(var, c) or not len(var.domain):
    #                     unsatisfied += 1
    #     return unsatisfied

    def countUnsatisfiedConstraints(self, state):
           unsatisfied = 0
           varList = state.viList
           for var in varList: 
               if len(var.domain) != 1:
                   unsatisfied += 1
           return unsatisfied


    # Needed for Graph
    def getGoal(self):
        return None


    def countInconsistentDomainValues(self, x, c):
        pairs = []
        nofInconsistency = 0
        for k in c.variables:
            for value in x.domain:
                pairs.extend( list(itertools.product([value], k.domain)) )
        
        for p in pairs :
            if not c.constraint( p[0], p[1] ):
                nofInconsistency += 1

        return nofInconsistency
 def __init__(self, variables, domains, expressions):
     super(Astar_GAC, self).__init__()
     self.cnet = CNET(variables, domains, expressions)
     self.currentState = self.initializeState(self.cnet)
     self.gac = GAC(self.currentState)
     self.Astar = AStar(self)
 def A(m, h=0):
     a = AStar(m, h)
     for i in a.step():
         pass
     return None if not a.path else True
    def write_file(self, name, closed_set):
        """
		Escrita de um ficheiro .txt para o armazenamento dos
		pontos resultantes do algoritmo

		@param name nome do ficheiro .txt onde serao armazenados os dados
		@closed_set lista de pontos resultantes do algoritmo
		"""
        data = open(name, "w")
        for x in closed_set:
            data.writelines((str(x[0]), " , ", str(x[1]), "\n"))
        data.close()


if __name__ == "__main__":
    open_image = OpenImage("peppersgrad.pgm")
    a_star = AStar()
    open_image.open_image(a_star.START_POINT, a_star.END_POINT)
    start = time.time()
    a_star.a_star(open_image.img)
    finish = time.time() - start
    for point in a_star.closed_set:
        open_image.draw_point(point)
    print "Tempo de execucao: %.15f" % finish
    print "Iteracoes:", a_star.count
    print "Media por cada iteracao: %.15f" % (finish / a_star.count)
    open_image.show_image()
    open_image.save_image("result.png")
    write_file = WriteFile()
    write_file.write_file("data.txt", a_star.closed_set)
class Window:
    
    def create_grid(self, rows, columns):
        self.bfs_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen)
        self.dfs_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen)
        self.astar_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen)

    def update_cell(self, row, column, state):
    	self.bfs_grid.update_cell(row, column, state)
    	self.dfs_grid.update_cell(row, column, state)
    	self.astar_grid.update_cell(row, column, state)        

    def create_astar(self):
        self.bfs = AStar(self.bfs_grid, 'BFS')
        self.dfs = AStar(self.dfs_grid, 'DFS')
        self.astar = AStar(self.astar_grid, 'AStar')

        self.active_search = self.bfs
        self.active_grid = self.bfs_grid

    def create_buttons(self):
        l = 10
        h = 25
        t = self.height*2//3 + 10
        w = (self.width-l*4)//3

        p = [(l,t,w,h),(l*2 + w,t, w,h),(l*3 + w*2, t,w, h)]

        self.buttons = [pygbutton.PygButton(p[0], "BFS"), pygbutton.PygButton(p[1], "DFS"),
           pygbutton.PygButton(p[2], "A*")]


    def __init__(self, width=500,height=750):
        pygame.init()
        self.width = width
        self.height = height
        self.screen = pygame.display.set_mode((self.width, self.height))
        self.create_buttons()
        # self.font = pygame.font.SysFont('Arial', 25)
        self.font = pygame.font.Font('freesansbold.ttf', 16)


        self.WHITE = (255, 255, 255)
        self.BLACK = (0, 0, 0)

    def show_text(self):
        x = 10
        y = self.height*2//3 + 50
        text = self.bfs.getStats()
        self.screen.blit(self.font.render(text, True, self.BLACK), (x, y))
        y += 25
        text = self.dfs.getStats()
        self.screen.blit(self.font.render(text, True, self.BLACK), (x, y))
        y += 25
        text = self.astar.getStats()
        self.screen.blit(self.font.render(text, True, self.BLACK), (x, y))


    def drawPath(self):
        self.active_search.backtrackPath()
        pathNodes = self.active_search.bestPath
        for node in pathNodes[:-1]:
            ((x,y),s) = node.getID()
            if node.state != 'goal':
                self.active_grid.drawPath(x, y)


    def loop(self):
        clock = pygame.time.Clock()
        results = [None, None, None]
        active_result_index = 0
        active_result = results[active_result_index]

        while True:
            pygame.event.pump()

            active_result = results[active_result_index]
            if active_result == None or active_result.state != 'goal':
                active_result = self.active_search.iterateAStar()
                results[active_result_index] = active_result

            self.screen.fill(self.WHITE)
            self.active_grid.draw()
            self.drawPath()
            self.show_text()

            for event in pygame.event.get():
                if 'click' in self.buttons[0].handleEvent(event):
                    self.active_search = self.bfs
                    self.active_grid = self.bfs_grid
                    active_result_index = 0
                if 'click' in self.buttons[1].handleEvent(event):
                    self.active_search = self.dfs
                    self.active_grid = self.dfs_grid
                    active_result_index = 1
                if 'click' in self.buttons[2].handleEvent(event):
                    self.active_search = self.astar
                    self.active_grid = self.astar_grid
                    active_result_index = 2
                if event.type == pygame.QUIT: 
                    sys.exit()

            for button in self.buttons:
                button.draw(self.screen)

            # pygame.display.update(changed_rectangles) is faster
            pygame.display.flip()
 
            # --- Limit to 60 frames per second
            clock.tick(60)
Beispiel #45
0
                '''
                grid_mdp = GridWorldMDP(map_struct['seed_map'], map_struct['goal'], 
                    map_struct['start'], map_struct['bridge_probabilities'], 
                    map_struct['bridge_locations'])

                init_value = {}
                for s in grid_mdp.states:
                    init_value[s.tostring()] = np.linalg.norm(s - grid_mdp.goal_state)

                mdp = MDP(grid_mdp.states, grid_mdp.valid_actions_function, grid_mdp.cost_function)
                #value_fcn = mdp.value_iteration(value = value_fcn, plot=True, world_size = 50)
                value_fcn = mdp.value_iteration(value = init_value, plot=True, world_size = 50)

                #set up dubins astar
                dub = dubins_astar(world_points, value_fcn)
                astar = AStar(motion_primitives, dub.cost_function, dub.heuristic,
                    dub.valid_edge, dub.state_equality, plot = False)

                astar_state = np.array([state['x'],state['y'],state['theta']])
            else:
                '''
                following_dist = 0.0
                temp_idx = dub.last_idx
                while following_dist < dub.look_ahead_dist
                    temp_idx -= 1
                    path_diff = numpy.array([,])
                    following_dist += np.linalg.norm(path_diff)
                index = temp_idx
                '''
                #ind_diff = np.abs(np.array(indices) - carrot_idx)
                #index = indices[np.argmin(ind_diff)]
                astar_state = np.array([state['x'],state['y'],state['theta']]) #path_states[index,:]
Beispiel #46
0
    def move(self):
        self.step_num += 1
        # if bomb in the neigbourhood
        if self.parent.target in neighbours((self.x, self.y), True):
            # The agent found the bomb
            self.bomb_location = self.parent.target
            if self.is_scout:
                # bomb deactivated
                self.parent.bomb_found = True
                return True
            else:
                self.color = QColor(200, 96, 109, 255)
                # the path is being reinitialized in
                # order to be show to the scout
                self.bomb_location = self.parent.target

        # if agent is scout and scout knows the bomb
        if self.bomb_location and self.is_scout:
            if self.path_to_bomb:
                (self.x, self.y) = self.path_to_bomb.pop()
                return True
            # create a shortcut
            target = self.bomb_location
            known_region = self.path
            start = (self.x, self.y)
            print 'calling astar'
            shortest_path = AStar()
            print 'will find the shortest path'
            self.path_to_bomb = shortest_path.find_path(start, target, known_region)
            return True

        # select to move randomly avoiding
        # to go to the last position
        order_list = ['u', 'r', 'd', 'l']
        random.shuffle(order_list)

        # get the available moves
        moves = []
        for order in order_list:
            if order == 'u':
                moves = self.up(moves)
            elif order == 'r':
                moves = self.right(moves)
            elif order == 'd':
                moves = self.down(moves)
            elif order == 'l':
                moves = self.left(moves)

        # if moves available
        if len(moves) != 0:
            # check if agents meet each other
            for agent in self.parent.agents:
                if (agent.x, agent.y) in neighbours((self.x, self.y)):
                    self.exchanges += 1
                    agent.path += self.path
                    self.path += agent.path
                    self.path = list(set(self.path))
                    agent.path = list(set(agent.path))
                    if (agent.is_scout) and (self.bomb_location) and (not agent.bomb_location):
                        agent.bomb_location = self.bomb_location
                        return True
                    else:
                        if self.bomb_location and not agent.bomb_location:
                            
                            agent.bomb_location = self.bomb_location
                            agent.color = QColor(200, 96, 109, 255)

            for m in moves:
                # The agent prefers to go to a
                # square he has not visited
                if m in self.path:
                    #we put this move at the end
                    moves.remove(m)
                    moves.append(m)
            (self.x, self.y) = moves[0]
            if moves[0] in self.path:
                self.path.remove((self.x, self.y))
            self.path.append((self.x, self.y))
        else:
            print 'cannot move'
            return False
        return True
Beispiel #47
0
        self.delta_state = delta_state

    def get_end_state(self, state):
        return state + self.delta_state 

delta_states = [np.array([ 1,  0]),
                np.array([ 1,  1]),
                np.array([ 0,  1]),
                np.array([-1,  1]),
                np.array([-1,  0]),
                np.array([-1, -1]),
                np.array([ 0, -1]),
                np.array([ 1, -1])]
    
motion_primitives = [motion_primitive(delta_state) for delta_state in delta_states]

def cost_function(state, motion_primitive):
    return np.linalg.norm(motion_primitive.delta_state)

astar = AStar(motion_primitives, cost_function, heuristic, valid_edge_function, state_equality)
plan = astar.plan(start_state, goal_state)

pathx = [state[0] for state in plan]
pathy = [state[1] for state in plan]

plt.plot(pathx,pathy)
plt.plot([0, 7],[3, 3])
plt.plot([3, 10],[6, 6])

plt.show()
def A(m, h=0):
    a = AStar(m, h)
    for i in a.step():
        pass
    if not a.path:
        raise Exception("\a[!] A* PATH NOT FOUND!")
Beispiel #49
0
    def get_path(self, flags, start, end):

        astar = AStar(flags, start, end)
        astar.process()
        return astar.path()
Beispiel #50
0
def amiCoSimulator():
	"""simple AmiCo env interaction"""
	print "Py: AmiCo Simulation Started:"
	print "library found: "
	#add syspath
	new_path = os.path.abspath("./").lower()
	java_path = os.path.abspath("C:\\Program Files (x86)\\Java\\jre6\\bin").lower()
	java_path2 = os.path.abspath("C:\\Program Files (x86)\\Java\\jre6\\bin\\client").lower()
	sys.path.append(new_path)
	sys.path.append(java_path)
	sys.path.append(java_path2)
	
	if (sys.platform == 'linux2'):
	##########################################
	# find_library on Linux could only be used if your libAmiCoPyJava.so is
	# on system search path or path to the library is added in to LD_LIBRARY_PATH
	#
	# name =  'AmiCoPyJava'
		# loadName = find_library(name)
		##########################################
		loadName = './libAmiCoPyJava.so'
		libamico = ctypes.CDLL(loadName)
		print libamico
	else: #else if OS is a Mac OS X (libAmiCo.dylib is searched for) or Windows (AmiCo.dll)
		name =  'AmiCoPyJava.DLL'
		loadName = find_library(name)
		print loadName
		libamico = ctypes.CDLL(name)
		print libamico
	
	javaClass = "ch/idsia/benchmark/mario/environments/MarioEnvironment"
	libamico.amicoInitialize(1, "-Djava.class.path=." + os.pathsep +":../lib/jdom.jar")
	libamico.createMarioEnvironment(javaClass)
	
	reset = cfunc('reset', libamico, None, ('list', ListPOINTER(c_int), 1))
	getEntireObservation = cfunc('getEntireObservation', libamico, py_object,
								 ('list', c_int, 1),
								 ('zEnemies', c_int, 1))
	performAction = cfunc('performAction', libamico, None, ('list', ListPOINTER(c_int), 1))
	getEvaluationInfo = cfunc('getEvaluationInfo', libamico, py_object)
	getObservationDetails = cfunc('getObservationDetails', libamico, py_object)
	
	agent =AStar()

	options = ""
	if len(sys.argv) > 1:
		options = sys.argv[1]

	print "options: ", options
	k = 1
	seed =0
	print "Py: ======Evaluation STARTED======"
	totalIterations = 0
	for i in range(k, k+100):
		options1 = options + "-ls " + str(seed)+" -fps 30 -ld 0"
		reset(options1)
		obsDetails = getObservationDetails()
		agent.setObservationDetails(obsDetails[0], obsDetails[1], obsDetails[2], obsDetails[3])
		while (not libamico.isLevelFinished()):
			totalIterations +=1 
			libamico.tick();
			obs = getEntireObservation(1, 0)
			agent.integrateObservation(obs[0], obs[1], obs[2], obs[3], obs[4]);
			action = agent.getAction()
			performAction(action);
		evaluationInfo = getEvaluationInfo()
		print evaluationInfo
		seed += 1