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)
