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])
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)
