Beispiel #1
0
class Application:
    def __init__(self, map_name, human=True, fps=DEFAULT_FPS):
        self.running = False
        self.displaySurface = None
        self.config = configparser.ConfigParser()
        self.config.read(CONFIG_FILE)
        self.fps = fps
        self.__human = human
        self.clock = pygame.time.Clock()
        self.trajectory = []

        # Parse config file
        self.windowTitle = "CS440 MP2 Robotic Arm"
        self.window = eval(self.config.get(map_name, 'Window'))

        armBase = eval(self.config.get(map_name, 'ArmBase'))
        armLinks = eval(self.config.get(map_name, 'ArmLinks'))
        self.armLimits = [(0, 0), (0, 0), (0, 0)]
        for i in range(len(armLinks)):
            self.armLimits[i] = armLinks[i][-1]
        self.arm = Arm(armBase, armLinks)

        self.obstacles = eval(self.config.get(map_name, 'Obstacles'))
        self.goals = eval(self.config.get(map_name, 'Goals'))

    # Initializes the pygame context and certain properties of the maze
    def initialize(self):

        pygame.init()
        self.displaySurface = pygame.display.set_mode(
            (self.window[0], self.window[1]), pygame.HWSURFACE)
        self.displaySurface.fill(WHITE)
        pygame.display.flip()
        pygame.display.set_caption(self.windowTitle)
        self.running = True

    # Once the application is initiated, execute is in charge of drawing the game and dealing with the game loop
    def execute(self, searchMethod, granularity, trajectory, saveImage,
                saveMaze):
        self.initialize()
        if not self.running:
            print("Program init failed")
            raise SystemExit

        currAngle = [0, 0, 0]
        for i in range(len(self.arm.getArmAngle())):
            currAngle[i] = self.arm.getArmAngle()[i]
        self.gameLoop()

        if not self.__human:
            print("Transforming a map configuration to a maze...")
            maze = transformToMaze(self.arm, self.goals, self.obstacles,
                                   self.window, granularity)
            print("Done!")
            print("Searching the path...")
            path, num_explored = search(maze, searchMethod)
            for i in range(len(path)):
                self.arm.setArmAngle(path[i])
                if (trajectory > 0) and (i % trajectory == 0):
                    self.trajectory.append(self.arm.getArmPos())
                self.gameLoop()
            print("Done!")

        while self.running:
            pygame.event.pump()
            keys = pygame.key.get_pressed()

            if (keys[K_ESCAPE]):
                self.running = False

            if self.__human:
                alpha, beta, gamma = currAngle
                if (keys[K_z]):
                    alpha += granularity if isValueInBetween(
                        self.armLimits[ALPHA], alpha + granularity) else 0

                if (keys[K_x]):
                    alpha -= granularity if isValueInBetween(
                        self.armLimits[ALPHA], alpha - granularity) else 0

                if (keys[K_a]):
                    beta += granularity if isValueInBetween(
                        self.armLimits[BETA], beta + granularity) else 0

                if (keys[K_s]):
                    beta -= granularity if isValueInBetween(
                        self.armLimits[BETA], beta - granularity) else 0

                if (keys[K_q]):
                    gamma += granularity if isValueInBetween(
                        self.armLimits[GAMMA], gamma + granularity) else 0

                if (keys[K_w]):
                    gamma -= granularity if isValueInBetween(
                        self.armLimits[GAMMA], gamma - granularity) else 0

                newAngle = (alpha, beta, gamma)
                tempArm = copy.deepcopy(self.arm)
                tempArm.setArmAngle(newAngle)
                armEnd = tempArm.getEnd()
                armPos = tempArm.getArmPos()

                if doesArmTouchObstacles(
                        armPos, self.obstacles) or not isArmWithinWindow(
                            armPos, self.window):
                    continue

                if not doesArmTouchGoals(armEnd,
                                         self.goals) and doesArmTouchObstacles(
                                             armPos, self.goals):
                    continue

                self.arm.setArmAngle(newAngle)
                self.gameLoop()
                currAngle = copy.deepcopy(newAngle)

                if doesArmTouchGoals(armEnd, self.goals):
                    self.gameLoop()
                    print("SUCCESS")
                    raise SystemExit

        if saveImage:
            pygame.image.save(self.displaySurface, saveImage)

        if saveMaze and not self.__human:
            print(saveMaze)
            maze.saveToFile(saveMaze)
        print(saveMaze)

    def gameLoop(self):
        self.clock.tick(self.fps)
        self.displaySurface.fill(WHITE)
        self.drawTrajectory()
        self.drawArm()
        self.drawObstacles()
        self.drawGoal()
        pygame.display.flip()

    def drawTrajectory(self):
        cnt = 1
        for armPos in self.trajectory:
            x = (255 - 255 / len(self.trajectory) * cnt)
            color = (x, x, x)
            cnt += 1
            for i in range(len(armPos)):
                pygame.draw.line(self.displaySurface, color, armPos[i][0],
                                 armPos[i][1], ARM_LINKS_WIDTH[i])

    def drawArm(self):
        armPos = self.arm.getArmPos()
        for i in range(len(armPos)):
            pygame.draw.line(self.displaySurface, BLACK, armPos[i][0],
                             armPos[i][1], ARM_LINKS_WIDTH[i])

    def drawObstacles(self):
        for obstacle in self.obstacles:
            pygame.draw.circle(self.displaySurface, RED,
                               (obstacle[0], obstacle[1]), obstacle[2])

    def drawGoal(self):
        for goal in self.goals:
            pygame.draw.circle(self.displaySurface, BLUE, (goal[0], goal[1]),
                               goal[2])
Beispiel #2
0
def transformToMaze(arm, goals, obstacles, window, granularity):
    """This function transforms the given 2D map to the maze in MP1.
    
        Args:
            arm (Arm): arm instance
            goals (list): [(x, y, ,z,r)] of goals
            obstacles (list): [(x, y, z,r)] of obstacles
            window (tuple): (width, height,length) of the window
            granularity (int): unit of increasing/decreasing degree for angles

        Return:
            Maze: the maze instance generated based on input arguments.

    """
    numLims = Arm.getNumArmLinks(arm)
    alpha_lims = (0, 0)
    beta_lims = (0, 0)
    gamma_lims = (0, 0)
    limits = Arm.getArmLimit(arm)
    #print("limits",limits)
    #print(len(limits))
    for i in range(len(limits)):
        if i == 0:
            alpha_lims = limits[i]
        elif i == 1:
            beta_lims = limits[i]
        elif i == 2:
            gamma_lims = limits[i]
    #print("gamma",gamma_lims)
    alpha = int((alpha_lims[1] - alpha_lims[0]) / granularity) + 1
    beta = int((beta_lims[1] - beta_lims[0]) / granularity) + 1
    gamma = int((gamma_lims[1] - gamma_lims[0]) / granularity) + 1
    start = Arm.getArmAngle(arm)
    if len(start) == 1:
        start = (start[0], 0, 0)
    elif len(start) == 2:
        start = (start[0], start[1], 0)
    startIdx = angleToIdx([start[0], start[1], start[2]],
                          [alpha_lims[0], beta_lims[0], gamma_lims[0]],
                          granularity)
    #print(startIdx)
    map = []
    for a in range(alpha):
        beta_row = []
        for b in range(beta):  #beta_row.append(SPACE_CHAR)
            gamma_row = []
            for c in range(gamma):
                gamma_row.append(SPACE_CHAR)
            beta_row.append(gamma_row)
        map.append(beta_row)
    #print(len(map),len(map[0]))
    for a in range(alpha_lims[0], alpha_lims[1] + granularity, granularity):
        for b in range(beta_lims[0], beta_lims[1] + granularity, granularity):
            for c in range(gamma_lims[0], gamma_lims[1] + granularity,
                           granularity):
                Arm.setArmAngle(arm, (a, b, c))
                armPos = Arm.getArmPos(arm)
                armEnd = Arm.getEnd(arm)
                armPosDist = Arm.getArmPosDist(arm)
                index = angleToIdx(
                    [a, b, c], [alpha_lims[0], beta_lims[0], gamma_lims[0]],
                    granularity)
                if map[index[0]][index[1]][index[2]] != SPACE_CHAR:
                    continue
                #print(index)
                touchObstacles = doesArmTouchObjects(armPosDist, obstacles,
                                                     False)
                touchGoals = doesArmTouchObjects(armPosDist, goals, True)
                if index[0] is startIdx[0] and index[1] is startIdx[
                        1] and index[2] is startIdx[2]:
                    map[index[0]][index[1]][index[2]] = START_CHAR  #
                elif not isArmWithinWindow(armPos,
                                           window) or touchObstacles[0]:
                    map[index[0]][index[1]][index[2]] = WALL_CHAR
                    if touchObstacles[1] == armPosDist[0]:
                        for shortBeta in range(beta_lims[0],
                                               beta_lims[1] + granularity,
                                               granularity):
                            for shortGamma in range(
                                    gamma_lims[0], gamma_lims[1] + granularity,
                                    granularity):
                                shortIndex = angleToIdx(
                                    [a, shortBeta, shortGamma], [
                                        alpha_lims[0], beta_lims[0],
                                        gamma_lims[0]
                                    ], granularity)
                                map[shortIndex[0]][shortIndex[1]][
                                    shortIndex[2]] = WALL_CHAR
                    if len(armPosDist
                           ) >= 2 and touchObstacles[1] == armPosDist[1]:
                        for shortGamma in range(gamma_lims[0],
                                                gamma_lims[1] + granularity,
                                                granularity):
                            shortIndex = angleToIdx(
                                [a, b, shortGamma],
                                [alpha_lims[0], beta_lims[0], gamma_lims[0]],
                                granularity)
                            map[shortIndex[0]][shortIndex[1]][
                                shortIndex[2]] = WALL_CHAR
                elif touchGoals[0] and not doesArmTipTouchGoals(
                        armEnd, goals
                ):  # goals works fine, just need to add more walls
                    map[index[0]][index[1]][index[2]] = WALL_CHAR
                    if touchObstacles[1] == armPosDist[0]:
                        for shortBeta in range(beta_lims[0],
                                               beta_lims[1] + granularity,
                                               granularity):
                            for shortGamma in range(
                                    gamma_lims[0], gamma_lims[1] + granularity,
                                    granularity):
                                shortIndex = angleToIdx(
                                    [a, shortBeta, shortGamma], [
                                        alpha_lims[0], beta_lims[0],
                                        gamma_lims[0]
                                    ], granularity)
                                map[shortIndex[0]][shortIndex[1]][
                                    shortIndex[2]] = WALL_CHAR
                    if len(armPosDist
                           ) >= 2 and touchObstacles[1] == armPosDist[1]:
                        for shortGamma in range(gamma_lims[0],
                                                gamma_lims[1] + granularity,
                                                granularity):
                            shortIndex = angleToIdx(
                                [a, b, shortGamma],
                                [alpha_lims[0], beta_lims[0], gamma_lims[0]],
                                granularity)
                            map[shortIndex[0]][shortIndex[1]][
                                shortIndex[2]] = WALL_CHAR
                elif touchGoals[0] and doesArmTipTouchGoals(armEnd, goals):
                    map[index[0]][index[1]][index[2]] = OBJECTIVE_CHAR
                    if touchObstacles[1] == armPosDist[0]:
                        for shortBeta in range(beta_lims[0],
                                               beta_lims[1] + granularity,
                                               granularity):
                            for shortGamma in range(
                                    gamma_lims[0], gamma_lims[1] + granularity,
                                    granularity):
                                shortIndex = angleToIdx(
                                    [a, shortBeta, shortGamma], [
                                        alpha_lims[0], beta_lims[0],
                                        gamma_lims[0]
                                    ], granularity)
                                map[shortIndex[0]][shortIndex[1]][
                                    shortIndex[2]] = OBJECTIVE_CHAR
                    if len(armPosDist
                           ) >= 2 and touchObstacles[1] == armPosDist[1]:
                        for shortGamma in range(gamma_lims[0],
                                                gamma_lims[1] + granularity,
                                                granularity):
                            shortIndex = angleToIdx(
                                [a, b, shortGamma],
                                [alpha_lims[0], beta_lims[0], gamma_lims[0]],
                                granularity)
                            map[shortIndex[0]][shortIndex[1]][
                                shortIndex[2]] = OBJECTIVE_CHAR
    return Maze(map, [alpha_lims[0], beta_lims[0], gamma_lims[0]], granularity)