def Transform_xy(self, measurement):
# 0 is box, wall, warehouse
# 1 is Mark (Direction)
# 2 is Dist
# 3 is bearing in Radian
newAngle = robot.truncate_angle(measurement[3] + self.robot.bearing)
x = self.robot.x + measurement[2] * math.cos(newAngle)
y = self.robot.y + measurement[2] * math.sin(newAngle)
return [x,y]
def _attempt_down(self, relative_bearing):
# The robot will attempt to place the box so that its center point
# is at this bearing relative to the robot orientation and at a distance of 0.5 units away.
# If the box can be so placed without intersecting
# anything, it does so. Otherwise, the placement fails and the robot continues to hold the box.
# The cost to set a box down is 1.5 (regardless of the direction in which the robot puts down the box).
# - If a box is placed on the '@' space, it is considered delivered and is removed from the ware-
# house.
# Illegal moves (do not set box down but incur cost):
# - putting down a box too far away or so that it's touching a wall, the warehouse exterior,
# another box, or the robot
# - putting down a box while not holding a box
try:
self._increase_total_cost_by(self.BOX_DOWN_COST)
# find the bearing relative to the warehouse
bearing = robot.truncate_angle(relative_bearing +
self.robot.bearing)
# the x and y coordinates in the warehouse
x = self.robot.x + 0.5 * math.cos(bearing)
y = self.robot.y + 0.5 * math.sin(bearing)
destination = (x, y)
destination_is_open = self._is_open(destination, self.BOX_SIZE)
destination_is_within_warehouse = self._is_within_warehouse(
destination, self.BOX_SIZE)
robot_has_box = self._robot_has_box()
is_legal_down = (destination_is_open
and destination_is_within_warehouse
and robot_has_box)
if is_legal_down:
self._down_box(destination)
elif VERBOSE_FLAG:
if not destination_is_open:
print "Could not set box down because destination is not open"
if not destination_is_within_warehouse:
print "Could not set box down because destination is not within warehouse"
if not robot_has_box:
print "Could not set box down because robot is not holding a box"
except ValueError:
raise Exception('improperly formatted down bearing: {}'.format(
relative_bearing))
def _attempt_lift(self, relative_bearing):
# If a box's center point is within 0.5 units of the robot's center point and its bearing
# is within 0.2 radians of the specified bearing, then the box is lifted. Otherwise, nothing
# happens.
# - The cost to pick up a box is 2, regardless of the direction the box is relative to the robot.
# - While holding a box, the robot may not pick up another box.
# Illegal lifts (do not lift a box but incur cost of lift):
# - picking up a box that doesn't exist or is too far away
# - picking up a box while already holding one
try:
self._increase_total_cost_by(self.BOX_LIFT_COST)
legal_lift = False
if not self._robot_has_box():
# process all boxes still remaining and lift the first one
# close enough to be lifted
for box_id in self.boxes:
box_position = (self.boxes[box_id]['ctr_x'],
self.boxes[box_id]['ctr_y'])
(distance,
brg) = self.robot.measure_distance_and_bearing_to(
box_position)
if math.fabs(robot.truncate_angle(brg - relative_bearing)
) <= 0.2 and distance <= 0.5:
self._lift_box(box_id)
legal_lift = True
break
if not legal_lift and VERBOSE_FLAG:
if self._robot_has_box():
print "*** Could not lift box because robot_has_box = {} ".format(
self._robot_has_box)
else:
print "*** Could not lift box because no box is close enough"
except ValueError:
raise Exception('improperly formatted lift bearing: {}'.format(
relative_bearing))
def plan_delivery(self):
## Make the Grid world with 20x20 resolution
#print "Height of the warehouse:",len(self.warehouse)
#print "Width of the warehouse:",len(self.warehouse[0])
# print(" ")
# print("inputWarehouse")
# for i in range(len(self.warehouse)):
# print(self.warehouse[i])
grid_size = 10
moves = []
newWarehouse = [[
'.' for i in range(grid_size * len(self.warehouse[0]))
] for j in range(grid_size * len(self.warehouse))]
#print " "
#print "newEmptytWarehouse"
#for i in range(len(newWarehouse)):
# print newWarehouse[i]
walls = []
drop_zone = []
for i in range(len(self.warehouse)):
for j in range(len(self.warehouse[0])):
if self.warehouse[i][j] == '#':
walls.append([i, j])
elif self.warehouse[i][j] == '@':
drop_zone.append([i, j])
packages = self.todo
expanded_pkgs = []
for pkg in packages:
expanded_pkgs.append(
[abs(int(pkg[1] * grid_size)),
int(pkg[0] * grid_size)])
#print " "
#print "packages at: ", expanded_pkgs
#print " "
#print "Walls at: ", walls
#print " "
#print "drop_zone at: ", drop_zone
newWalls = []
for wall in walls:
for i in range(-int(0.3 * grid_size),
grid_size + int(0.3 * grid_size)):
for j in range(-int(0.3 * grid_size),
grid_size + int(0.3 * grid_size)):
newWalls.append([(wall[0] * grid_size) + i,
(wall[1] * grid_size) + j])
new_drop_zone = []
for drop in drop_zone:
for i in range(0, grid_size):
for j in range(0, grid_size):
new_drop_zone.append([(drop[0] * grid_size) + i,
(drop[1] * grid_size) + j])
#print " "
#print "NewWalls at: ", newWalls
robot_init_pos = [
math.ceil((2 * (drop_zone[0][0] * grid_size) + grid_size) / 2),
math.ceil((2 * (drop_zone[0][1] * grid_size) + grid_size) / 2)
]
#print " "
#print "Robot initial position"
#print robot_init_pos
## Wall padding
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ((i < int(0.3 * grid_size))
or (i >= len(newWarehouse) - int(0.3 * grid_size))):
newWarehouse[i][j] = '#'
if ((j < int(0.3 * grid_size))
or (j >= len(newWarehouse[0]) - int(0.3 * grid_size))):
newWarehouse[i][j] = '#'
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ([i, j] in newWalls):
newWarehouse[i][j] = '#'
if ([i, j] in new_drop_zone):
newWarehouse[i][j] = '@'
if ([i, j] == robot_init_pos):
newWarehouse[i][j] = '*'
if ([i, j] in expanded_pkgs):
for k in range(int(0.3 * grid_size)):
for l in range(int(0.3 * grid_size)):
newWarehouse[i][j] = '#'
newWarehouse[i][j + l] = '#'
newWarehouse[i + k][j] = '#'
newWarehouse[i + k][j + l] = '#'
newWarehouse[i - k][j + l] = '#'
newWarehouse[i + k][j - l] = '#'
newWarehouse[i][j - l] = '#'
newWarehouse[i - k][j] = '#'
newWarehouse[i - k][j - l] = '#'
print(" ")
print("ExpandedWarehouse")
for i in range(len(newWarehouse)):
print(newWarehouse[i])
## Movement Cost
cost = 1
delta = [
[-1, 0], # go up
[0, -1], # go left
[1, 0], # go down
[0, 1], # go right
[-1, 1], # go top right
[-1, -1], # go top left
[1, 1], # go down right
[1, -1], # go down left
]
## Helper Functions
## Heuristic Function
def compute_heu(grid, goal, delta, cost):
def inGrid(grid, x, y):
if x >= 0 and x < len(grid[0]) and y >= 0 and y < len(grid):
return True
else:
return False
value = [[99999 for x in range(len(grid[0]))]
for y in range(len(grid))]
value[goal[0]][goal[1]] = 0
openList = []
openList.append([0, goal[0], goal[1]])
while len(openList) != 0:
openList.sort()
currentCell = openList.pop(0)
for i in range(len(delta)):
targetX = currentCell[2] + delta[i][1]
targetY = currentCell[1] + delta[i][0]
if inGrid(grid, targetX, targetY):
if (grid[targetY][targetX] == '.'
or grid[targetY][targetX]
== '@') and value[targetY][targetX] == 99999:
openList.append([(currentCell[0] + cost + int(
3 *
distance_between(goal, [targetX, targetY]))),
targetY, targetX])
value[targetY][targetX] = (
currentCell[0] + cost +
int(3 * distance_between(
goal, [targetX, targetY])))
return value
## Search Function
def search(grid, init, goal, cost, heuristic, moves):
closed = [[0 for col in range(len(grid[0]))]
for row in range(len(grid))]
for i in range(len(heuristic)):
for j in range(len(heuristic)):
if (heuristic[i][j] == 99999):
closed[i][j] = 1
#closed[init[0]][init[1]] = 1
#for i in range(len(closed)):
# for j in range(len(closed[0])):
# if(closed[i][j] != 99999):
# closed[i][j] = 0
# else:
# closed[i][j] = 1
# print(" ")
# print("closed array in the beginning")
# for i in range(len(closed[0])):
# print((closed[i]))
expand = [[-1 for col in range(len(grid[0]))]
for row in range(len(grid))]
for i in range(len(heuristic)):
for j in range(len(heuristic)):
if (heuristic[i][j] == 99999):
expand[i][j] = -9
# expand = copy.copy(heuristic)
# for i in range(len(expand)):
# for j in range(len(expand[0])):
# if(expand[i][j] != 99999):
# expand[i][j] = -1
# if(expand[i][j] == 0):
# expand[i][j] = -99
#print(" ")
#print("expand array in the beginning")
#for i in range(len(expand[0])):
# print(expand[i])
#action = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))]
x = init[0]
y = init[1]
g = 0
f = g + heuristic[x][y]
#print(" ")
#print("heuristic array in the search")
#for i in range(len(heuristic[0])):
# print(heuristic[i])
open_cells = [[f, g, x, y]]
#print("")
#print("open_cells:")
#print(open_cells)
found = False # flag that is set when search is complete
resign = False # flag set if we can't find expand
count = 0
while not found and not resign:
if len(open_cells) == 0:
resign = True
return moves
else:
open_cells.sort()
open_cells.reverse()
next_cell = open_cells.pop()
#print("")
#print("Next_cell")
#print(next_cell)
x = next_cell[2]
y = next_cell[3]
g = next_cell[1]
f = next_cell[0]
expand[x][y] = count
if (count > 0):
mv = 'move ' + str(x) + ' ' + str(y)
moves.append(mv)
count += 1
if distance_between([x, y], [
goal[0], goal[1]
]) <= 0.45 * grid_size or distance_between(
[x, y],
[goal[0], goal[1]]) == math.sqrt((0.4 * grid_size)**2 +
(0.4 * grid_size)**2):
found = True
print("")
print("expand")
for i in range(len(expand)):
print((expand[i]))
return moves
else:
for i in range(len(delta)):
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(
grid) and y2 >= 0 and y2 < len(grid[0]):
if closed[x2][y2] == 0 and (grid[x2][y2] == '.'
or grid[x2][y2]
== '@'):
g2 = g + cost
f2 = g2 + heuristic[x2][y2]
open_cells.append([f2, g2, x2, y2])
#print("")
#print("after appending open_cells")
#print(open_cells)
closed[x2][y2] = 1
print("")
print("expand")
for i in range(len(expand[0])):
print((expand[i]))
return moves
def distance_between(a, b):
i, j = a
p, q = b
dist = math.sqrt((i - p)**2 + (j - q)**2)
return dist
distances = []
for to in range(len(expanded_pkgs)):
ddd = distance_between(
[int(robot_init_pos[0]),
int(robot_init_pos[0])],
[int(expanded_pkgs[to][0]),
int(expanded_pkgs[to][0])])
distances.append([ddd, to])
distances.sort(reverse=True)
## For each package
for to in range(len(expanded_pkgs)):
#pos = distances.pop()
#to = pos[1]
pkg_pos = expanded_pkgs[to]
print(" ")
print(("Package:", to))
newWarehouse = [[
'.' for i in range(grid_size * len(self.warehouse[0]))
] for j in range(grid_size * len(self.warehouse))]
newWalls = []
for wall in walls:
for i in range(-int(0.3 * grid_size),
grid_size + int(0.3 * grid_size)):
for j in range(-int(0.3 * grid_size),
grid_size + int(0.3 * grid_size)):
newWalls.append([(wall[0] * grid_size) + i,
(wall[1] * grid_size) + j])
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ([i, j] in newWalls):
newWarehouse[i][j] = '#'
if ([i, j] in new_drop_zone):
newWarehouse[i][j] = '@'
if ([i, j] == robot_init_pos):
newWarehouse[i][j] = '*'
if ([i, j] not in [pkg_pos] and [i, j] in expanded_pkgs):
for k in range(int(0.3 * grid_size)):
for l in range(int(0.3 * grid_size)):
newWarehouse[i][j] = '#'
newWarehouse[i][j + l] = '#'
newWarehouse[i + k][j] = '#'
newWarehouse[i + k][j + l] = '#'
newWarehouse[i - k][j + l] = '#'
newWarehouse[i + k][j - l] = '#'
newWarehouse[i][j - l] = '#'
newWarehouse[i - k][j] = '#'
newWarehouse[i - k][j - l] = '#'
## Wall padding
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ((i < int(0.3 * grid_size)) or
(i >= len(newWarehouse) - int(0.3 * grid_size))):
newWarehouse[i][j] = '#'
if ((j < int(0.3 * grid_size)) or
(j >= len(newWarehouse[0]) - int(0.3 * grid_size))):
newWarehouse[i][j] = '#'
print(" ")
print("ExpandedWarehouse for package:", to)
for i in range(len(newWarehouse)):
print(newWarehouse[i])
## Move towards the package
heu = compute_heu(
newWarehouse,
[int(pkg_pos[0]), int(pkg_pos[1])], delta, cost)
# print(" ")
# print("Heuristic Grid")
# for i in range(len(heu[0])):
# print((heu[i]))
if (to == 0):
final_pos = [int(robot_init_pos[0]), int(robot_init_pos[1])]
moves = search(newWarehouse, final_pos,
[int(pkg_pos[0]), int(pkg_pos[1])], cost,
copy.copy(heu), moves)
## Grab the package
moves.append('lift ' + str(to))
newWarehouse[int(pkg_pos[0])][int(pkg_pos[1])] = newWarehouse[int(
pkg_pos[0])][int(pkg_pos[1])].replace('$', '.')
for i in range(len(moves)):
mov = moves[-(i + 1)].split()
if (mov[0] == 'move'):
final_pos = int(mov[1]), int(mov[2])
break
## Move towards the drop_zone
heu = compute_heu(newWarehouse,
[int(robot_init_pos[0]),
int(robot_init_pos[1])], delta, cost)
# print(" ")
# print("Heuristic Grid")
# for i in range(len(heu[0])):
# print((heu[i]))
moves = search(newWarehouse, final_pos,
[int(robot_init_pos[0]),
int(robot_init_pos[1])], cost, heu, moves)
## Drop the Package
moves.append('down ' + str(int(robot_init_pos[1]) / grid_size) +
' ' + str(int(robot_init_pos[0]) / grid_size))
for i in range(len(moves)):
mov = moves[-(i + 1)].split()
if (mov[0] == 'move'):
final_pos = int(mov[1]), int(mov[2])
break
print(moves)
cal_moves = []
def truncate_angle(t):
return ((t + PI) % (2 * PI)) - PI
myrobot = robot.Robot(x=robot_init_pos[1] / grid_size,
y=-robot_init_pos[0] / grid_size,
bearing=0.0,
max_distance=self.max_distance,
max_steering=self.max_steering)
myrobot.set_noise(steering_noise=0,
distance_noise=0,
measurement_noise=0)
#print("Robot's init pos:",myrobot)
robot_pos = myrobot.__repr__()
for move in moves:
mv = move.split()
if (mv[0] == 'lift'):
cal_moves.append(move)
if (mv[0] == 'down'):
cal_moves.append(move)
if (mv[0] == 'move'):
x, y = float(mv[2]) / grid_size, -float(mv[1]) / grid_size
#print("x=",x,"y=",y)
dist, bearing = myrobot.measure_distance_and_bearing_to([x, y])
target_bearing = myrobot.bearing + bearing
steer = robot.truncate_angle(target_bearing - myrobot.bearing)
steer = max(-self.max_steering, steer)
steer = min(self.max_steering, steer)
while (steer > 0.0000000000000006
or steer < -0.0000000000000006):
steer = robot.truncate_angle(target_bearing -
myrobot.bearing)
#print("steer",steer)
steer = max(-self.max_steering, steer)
steer = min(self.max_steering, steer)
m = "move " + str(steer) + " " + str(0)
cal_moves.append(m)
myrobot.move(steer, 0)
#print("robot bearing",myrobot.bearing)
#print("target bearing",target_bearing)
while (dist > 0.0000001):
dist, bearing = myrobot.measure_distance_and_bearing_to(
[x, y])
dist = max(0, dist)
dist = min(self.max_distance, dist)
#print("Distance to point",dist)
m = "move " + str(0) + " " + str(dist)
cal_moves.append(m)
myrobot.move(0, dist)
#print("robot position",myrobot.__repr__(),"Robot bearing:",myrobot.bearing)
#print("target position",[x,y])
# m = "move " + str(steer) + " " + str(dist)
# cal_moves.append(m)
# print("new dist:",dist,"new steer",steer)
# myrobot.move(steer,dist)
# print("new robot pos",myrobot)
# robot_pos = myrobot.__repr__()
return cal_moves
def plan_delivery(self):
grid_size = 10
moves = []
newWarehouse = [[
'.' for i in range(grid_size * len(self.warehouse[0]))
] for j in range(grid_size * len(self.warehouse))]
walls = []
drop_zone = []
for i in range(len(self.warehouse)):
for j in range(len(self.warehouse[0])):
if self.warehouse[i][j] == '#':
walls.append([i, j])
elif self.warehouse[i][j] == '@':
drop_zone.append([i, j])
packages = self.todo
expanded_pkgs = []
for pkg in packages:
expanded_pkgs.append(
[abs(int(pkg[1] * grid_size)),
int(pkg[0] * grid_size)])
newWalls = []
for wall in walls:
for i in range(-int(0.3 * grid_size),
grid_size + int(0.3 * grid_size)):
for j in range(-int(0.3 * grid_size),
grid_size + int(0.3 * grid_size)):
newWalls.append([(wall[0] * grid_size) + i,
(wall[1] * grid_size) + j])
new_drop_zone = []
for drop in drop_zone:
for i in range(0, grid_size):
for j in range(0, grid_size):
new_drop_zone.append([(drop[0] * grid_size) + i,
(drop[1] * grid_size) + j])
robot_init_pos = [
math.ceil((2 * (drop_zone[0][0] * grid_size) + grid_size) / 2),
math.ceil((2 * (drop_zone[0][1] * grid_size) + grid_size) / 2)
]
## Wall padding
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ((i < int(0.3 * grid_size))
or (i >= len(newWarehouse) - int(0.3 * grid_size))):
newWarehouse[i][j] = '#'
if ((j < int(0.3 * grid_size))
or (j >= len(newWarehouse[0]) - int(0.3 * grid_size))):
newWarehouse[i][j] = '#'
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ([i, j] in newWalls):
newWarehouse[i][j] = '#'
if ([i, j] in new_drop_zone):
newWarehouse[i][j] = '@'
if ([i, j] == robot_init_pos):
newWarehouse[i][j] = '*'
if ([i, j] in expanded_pkgs):
newWarehouse[i][j] = '$'
## Movement Cost
cost = 1
delta = [
[-1, 0], # go up
[0, -1], # go left
[1, 0], # go down
[0, 1], # go right
[-1, 1], # go top right
[-1, -1], # go top left
[1, 1], # go down right
[1, -1], # go down left
]
## Helper Functions
## Heuristic Function
def compute_heu(grid, goal, delta, cost):
def inGrid(grid, x, y):
if x >= 0 and x < len(grid[0]) and y >= 0 and y < len(grid):
return True
else:
return False
value = [[99999 for x in range(len(grid[0]))]
for y in range(len(grid))]
value[goal[0]][goal[1]] = 0
openList = []
openList.append([0, goal[0], goal[1]])
while len(openList) != 0:
openList.sort()
currentCell = openList.pop(0)
for i in range(len(delta)):
targetX = currentCell[2] + delta[i][1]
targetY = currentCell[1] + delta[i][0]
if inGrid(grid, targetX, targetY):
if (grid[targetY][targetX] == '.'
or grid[targetY][targetX]
== '@') and value[targetY][targetX] == 99999:
openList.append([(currentCell[0] + cost + int(
3 *
distance_between(goal, [targetX, targetY]))),
targetY, targetX])
value[targetY][targetX] = (
currentCell[0] + cost +
int(3 * distance_between(
goal, [targetX, targetY])))
return value
## Search Function
def search(grid, init, goal, cost, heuristic, moves):
closed = [[0 for col in range(len(grid[0]))]
for row in range(len(grid))]
for i in range(len(heuristic)):
for j in range(len(heuristic)):
if (heuristic[i][j] == 99999):
closed[i][j] = 1
expand = [[-1 for col in range(len(grid[0]))]
for row in range(len(grid))]
#
x = init[0]
y = init[1]
g = 0
f = g + heuristic[x][y]
open_cells = [[f, g, x, y]]
found = False # flag that is set when search is complete
resign = False # flag set if we can't find expand
count = 0
while not found and not resign:
if len(open_cells) == 0:
resign = True
return moves
else:
open_cells.sort()
open_cells.reverse()
next_cell = open_cells.pop()
x = next_cell[2]
y = next_cell[3]
g = next_cell[1]
f = next_cell[0]
expand[x][y] = count
if (count > 0):
mv = 'move ' + str(x) + ' ' + str(y)
moves.append(mv)
count += 1
if distance_between(
[x, y], [goal[0], goal[1]]
) <= 0.4 * grid_size: # or distance_between([x,y],[goal[0],goal[1]]) == math.sqrt((0.4*grid_size)**2 +(0.4*grid_size)**2):
found = True
#
return moves
else:
for i in range(len(delta)):
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(
grid) and y2 >= 0 and y2 < len(grid[0]):
if closed[x2][y2] == 0 and (grid[x2][y2] == '.'
or grid[x2][y2]
== '@'):
g2 = g + cost
f2 = g2 + heuristic[x2][y2]
open_cells.append([f2, g2, x2, y2])
closed[x2][y2] = 1
return moves
def distance_between(a, b):
i, j = a
p, q = b
dist = math.sqrt((i - p)**2 + (j - q)**2)
return dist
distances = []
for to in range(len(expanded_pkgs)):
ddd = distance_between(
[int(robot_init_pos[0]),
int(robot_init_pos[0])],
[int(expanded_pkgs[to][0]),
int(expanded_pkgs[to][0])])
distances.append([ddd, to])
distances.sort(reverse=True)
## For each package
for to in range(len(expanded_pkgs)):
pkg_pos = expanded_pkgs[to]
newWarehouse = [[
'.' for i in range(grid_size * len(self.warehouse[0]))
] for j in range(grid_size * len(self.warehouse))]
newWalls = []
for wall in walls:
for i in range(-int(0.4 * grid_size),
grid_size + int(0.4 * grid_size)):
for j in range(-int(0.4 * grid_size),
grid_size + int(0.4 * grid_size)):
newWalls.append([(wall[0] * grid_size) + i,
(wall[1] * grid_size) + j])
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ([i, j] in newWalls):
newWarehouse[i][j] = '#'
if ([i, j] in new_drop_zone):
newWarehouse[i][j] = '@'
if ([i, j] == robot_init_pos):
newWarehouse[i][j] = '*'
if ([i, j] not in [pkg_pos] and [i, j] in expanded_pkgs):
for k in range(int(0.4 * grid_size)):
for l in range(int(0.4 * grid_size)):
newWarehouse[i][j] = '#'
newWarehouse[i][j + l] = '#'
newWarehouse[i + k][j] = '#'
newWarehouse[i + k][j + l] = '#'
newWarehouse[i - k][j + l] = '#'
newWarehouse[i + k][j - l] = '#'
newWarehouse[i][j - l] = '#'
newWarehouse[i - k][j] = '#'
newWarehouse[i - k][j - l] = '#'
## Wall padding
for i in range(len(newWarehouse)):
for j in range(len(newWarehouse[0])):
if ((i < int(0.4 * grid_size)) or
(i >= len(newWarehouse) - int(0.4 * grid_size))):
newWarehouse[i][j] = '#'
if ((j < int(0.4 * grid_size)) or
(j >= len(newWarehouse[0]) - int(0.4 * grid_size))):
newWarehouse[i][j] = '#'
## Move towards the package
heu = compute_heu(
newWarehouse,
[int(pkg_pos[0]), int(pkg_pos[1])], delta, cost)
if (to == 0):
final_pos = [int(robot_init_pos[0]), int(robot_init_pos[1])]
moves = search(newWarehouse, final_pos,
[int(pkg_pos[0]), int(pkg_pos[1])], cost,
copy.copy(heu), moves)
## Grab the package
moves.append('lift ' + str(to))
newWarehouse[int(pkg_pos[0])][int(pkg_pos[1])] = newWarehouse[int(
pkg_pos[0])][int(pkg_pos[1])].replace('$', '.')
for i in range(len(moves)):
mov = moves[-(i + 1)].split()
if (mov[0] == 'move'):
final_pos = int(mov[1]), int(mov[2])
break
## Move towards the drop_zone
heu = compute_heu(newWarehouse,
[int(robot_init_pos[0]),
int(robot_init_pos[1])], delta, cost)
moves = search(newWarehouse, final_pos,
[int(robot_init_pos[0]),
int(robot_init_pos[1])], cost, heu, moves)
## Drop the Package
moves.append('down ' + str(int(robot_init_pos[1]) / grid_size) +
' ' + str(int(-robot_init_pos[0]) / grid_size))
for i in range(len(moves)):
mov = moves[-(i + 1)].split()
if (mov[0] == 'move'):
final_pos = int(mov[1]), int(mov[2])
break
#
cal_moves = []
def truncate_angle(t):
return ((t + PI) % (2 * PI)) - PI
myrobot = robot.Robot(x=robot_init_pos[1] / grid_size,
y=-robot_init_pos[0] / grid_size,
bearing=0.0,
max_distance=self.max_distance,
max_steering=self.max_steering)
myrobot.set_noise(steering_noise=0,
distance_noise=0,
measurement_noise=0)
#print("Robot's init pos:",myrobot)
robot_pos = myrobot.__repr__()
for move in moves:
mv = move.split()
if (mv[0] == 'lift'):
cal_moves.append(move)
if (mv[0] == 'down'):
cal_moves.append(move)
if (mv[0] == 'move'):
x, y = float(mv[2]) / grid_size, -float(mv[1]) / grid_size
#print("x=",x,"y=",y)
dist, bearing = myrobot.measure_distance_and_bearing_to([x, y])
target_bearing = myrobot.bearing + bearing
steer = robot.truncate_angle(target_bearing - myrobot.bearing)
steer = max(-self.max_steering, steer)
steer = min(self.max_steering, steer)
while (steer > 0.0000000000000006
or steer < -0.0000000000000006):
steer = robot.truncate_angle(target_bearing -
myrobot.bearing)
#print("steer",steer)
steer = max(-self.max_steering, steer)
steer = min(self.max_steering, steer)
mm = "move " + str(steer) + " " + str(0)
cal_moves.append(mm)
myrobot.move(steer, 0)
while (dist > 0.000001):
dist, bearing = myrobot.measure_distance_and_bearing_to(
[x, y])
dist = max(0, dist)
dist = min(self.max_distance, dist)
mm = "move " + str(0) + " " + str(dist)
cal_moves.append(mm)
myrobot.move(0, dist)
# mod_moves = []
# for i in range(len(cal_moves)):
# if(cal_moves[i].split()[0] == 'move'):
# old_move = cal_moves[i].split()[1:]
# break
# mod_moves.append(cal_moves[i])
#
# for move in cal_moves[1:]:
#
# mv = move.split()
# if(mv[0]=='move'):
# new_move = mv[1:]
# if(new_move[0] == '0' and new_move[1] == '0'):
# continue
# if(new_move[0] == old_move[0]):
# if(float(new_move[1])+float(old_move[1]) < self.max_distance):
# agg_move = [new_move[0],str(float(new_move[1])+float(old_move[1]))]
# mmm = "move "+ agg_move[0]+ " " + agg_move[1]
# mod_moves.append(mmm)
#
# if(old_move[1] != new_move[1]):
# if((float(new_move[0])+float(old_move[0]))<=self.max_steering):
# agg_move = [str(float(new_move[0])+float(old_move[0])),new_move[1],]
# mmm = "move "+ agg_move[0]+ " " + agg_move[1]
# mod_moves.append(mmm)
# old_move = new_move
# else:
# mod_moves.append(move)
return cal_moves