def place_block(self, environment: env.map.Map):
"""
Move with the goal of placing a block.
This method is called if the current task is PLACE_BLOCK. If the agent has not planned a path yet,
it determines a path to descend from the current position to a position where it can let go of the block
to place it. In a more realistic simulation this placement process would likely be much more complex and
may indeed turn out to be one of the most difficult parts of the low-level quadcopter control necessary
for the construction task. In this case however, this complexity is not considered. Once the block has been
placed, if the structure is not finished the task becomes FETCH_BLOCK, otherwise it becomes LAND. Since this
is the global knowledge version of this algorithm, the agent also notifies all other agents of the change.
:param environment: the environment the agent operates in
"""
position_before = np.copy(self.geometry.position)
if self.current_path is None:
init_z = Block.SIZE * (
self.current_structure_level +
2) + self.required_spacing + self.geometry.size[2] / 2
first_z = Block.SIZE * (self.current_grid_position[2] +
2) + self.geometry.size[2] / 2
placement_x = Block.SIZE * self.current_grid_position[
0] + environment.offset_origin[0]
placement_y = Block.SIZE * self.current_grid_position[
1] + environment.offset_origin[1]
placement_z = Block.SIZE * (self.current_grid_position[2] +
1) + self.geometry.size[2] / 2
self.current_path = Path()
self.current_path.add_position([placement_x, placement_y, init_z])
self.current_path.add_position([placement_x, placement_y, first_z])
self.current_path.add_position(
[placement_x, placement_y, placement_z])
# check again whether attachment is allowed since other agents placing blocks there may have made it illegal
if not self.current_block_type_seed:
# check whether site is occupied
if environment.check_occupancy_map(self.current_grid_position):
self.current_path = None
self.current_task = Task.FIND_ATTACHMENT_SITE
self.task_history.append(self.current_task)
self.find_attachment_site(environment)
return
# check whether site is still legal
attachment_sites = legal_attachment_sites(
self.component_target_map[self.current_structure_level],
environment.occupancy_map[self.current_structure_level],
component_marker=self.current_component_marker)
if attachment_sites[self.current_grid_position[1],
self.current_grid_position[0]] == 0:
self.current_path = None
self.current_task = Task.FIND_ATTACHMENT_SITE
self.task_history.append(self.current_task)
self.find_attachment_site(environment)
return
if self.current_path.current_index != len(
self.current_path.positions) - 1:
next_position, current_direction = self.move(environment)
else:
next_position = self.current_path.next()
current_direction = self.current_path.direction_to_next(
self.geometry.position)
current_direction /= sum(np.sqrt(current_direction**2))
current_direction *= Agent.MOVEMENT_PER_STEP
self.per_task_step_count[self.current_task] += 1
if simple_distance(self.geometry.position,
next_position) <= Agent.MOVEMENT_PER_STEP:
self.geometry.position = next_position
ret = self.current_path.advance()
# block should now be placed in the environment's occupancy matrix
if not ret:
if self.current_block.geometry.position[2] > (
self.current_grid_position[2] + 1.0) * Block.SIZE:
self.aprint("Error: block placed in the air ({})".format(
self.current_grid_position[2]))
self.current_path.add_position(
np.array([
self.geometry.position[0],
self.geometry.position[1],
(self.current_grid_position[2] + 1) * Block.SIZE +
self.geometry.size[2] / 2
]))
return
if self.current_block.is_seed:
self.current_seed = self.current_block
self.components_seeded.append(
int(self.current_component_marker))
self.seeded_blocks += 1
else:
self.sp_search_count.append(
(self.current_sp_search_count,
int(self.current_component_marker),
self.current_task.name))
self.current_sp_search_count = 0
if self.current_component_marker not in self.components_attached:
self.components_attached.append(
int(self.current_component_marker))
self.attached_blocks += 1
if self.rejoining_swarm:
self.rejoining_swarm = False
self.attachment_frequency_count.append(
self.count_since_last_attachment)
self.count_since_last_attachment = 0
environment.place_block(self.current_grid_position,
self.current_block)
self.geometry.attached_geometries.remove(
self.current_block.geometry)
self.current_block.placed = True
self.current_block.grid_position = self.current_grid_position
self.current_block = None
self.current_path = None
self.current_visited_sites = None
self.transporting_to_seed_site = False
if self.check_structure_finished(self.local_occupancy_map) \
or (self.check_layer_finished(self.local_occupancy_map)
and self.current_structure_level >= self.target_map.shape[0] - 1):
self.current_task = Task.LAND
elif self.check_component_finished(self.local_occupancy_map):
self.current_task = Task.FETCH_BLOCK
else:
self.current_task = Task.FETCH_BLOCK
if self.current_block_type_seed:
self.current_block_type_seed = False
self.task_history.append(self.current_task)
for a in environment.agents:
a.recheck_task(environment)
else:
self.geometry.position = self.geometry.position + current_direction
self.per_task_distance_travelled[Task.PLACE_BLOCK] += simple_distance(
position_before, self.geometry.position)
def find_attachment_site(self, environment: env.map.Map):
"""
Move with the goal of finding an attachment site.
This method is called if the current task is FIND_ATTACHMENT_SITE. If this is the case, the agent is already
at the structure perimeter and knows its position in the grid. It then moves along the structure perimeter
(more specifically the perimeter of the current component) in counter-clockwise direction and determines
at each empty site it passes whether the carried block can be placed there. This is true at sites which should
be occupied according to the target occupancy matrix and which are 'inner corners' of the structure or
'end-of-row sites'. If the agent revisits a site during this, it is either 'stuck' in a hole and the next task
is MOVE_TO_PERIMETER to escape it, or the current component is complete in which case the agent switches to
FIND_NEXT_COMPONENT. If the agent does find an attachment site, the next task becomes PLACE_BLOCK.
:param environment: the environment the agent operates in
"""
position_before = np.copy(self.geometry.position)
if self.current_path is None:
self.current_path = Path()
self.current_path.add_position(self.geometry.position)
# use the following list to keep track of the combination of visited attachment sites and the direction of
# movement at that point in time; if, during the same "attachment search" one such "site" is revisited, then
# the agent is stuck in a loop, most likely caused by being trapped in a hole
if self.current_visited_sites is None:
self.current_visited_sites = []
next_position, current_direction = self.move(environment)
if simple_distance(self.geometry.position,
next_position) <= Agent.MOVEMENT_PER_STEP:
self.geometry.position = next_position
ret = self.current_path.advance()
if not ret:
self.update_local_occupancy_map(environment)
self.current_blocks_per_attachment += 1
# corner of the current block reached, assess next action
at_loop_corner, loop_corner_attachable = self.check_loop_corner(
environment, self.current_grid_position)
# check whether location is somewhere NW/NE/SW/SE of any closing corner, i.e. the block should
# not be placed there before closing that loop
allowable_region_attachable = True
if not at_loop_corner:
closing_corners = self.closing_corners[
self.current_structure_level][
self.current_component_marker]
for i in range(len(closing_corners)):
x, y, z = closing_corners[i]
orientation = self.closing_corner_orientations[
self.current_structure_level][
self.current_component_marker][i]
if not environment.check_occupancy_map(
np.array([x, y, z])):
if orientation == "NW":
if x >= self.current_grid_position[
0] and y <= self.current_grid_position[
1]:
allowable_region_attachable = False
break
elif orientation == "NE":
if x <= self.current_grid_position[
0] and y <= self.current_grid_position[
1]:
allowable_region_attachable = False
break
elif orientation == "SW":
if x >= self.current_grid_position[
0] and y >= self.current_grid_position[
1]:
allowable_region_attachable = False
break
elif orientation == "SE":
if x <= self.current_grid_position[
0] and y >= self.current_grid_position[
1]:
allowable_region_attachable = False
break
current_site_tuple = (tuple(self.current_grid_position),
tuple(self.current_grid_direction))
if current_site_tuple in self.current_visited_sites:
# there are two options here: either the current component is finished, or we are trapped in a hole
# check if in hole
if check_map(self.hole_map, self.current_grid_position,
lambda x: x > 1):
self.current_task = Task.MOVE_TO_PERIMETER
self.task_history.append(self.current_task)
self.current_grid_direction = [
1, 0, 0
] # should probably use expected SP here
self.local_occupancy_map[self.hole_boundaries[
self.hole_map[self.current_grid_position[2],
self.current_grid_position[1],
self.current_grid_position[0]]]] = 1
else:
self.local_occupancy_map[
self.component_target_map ==
self.current_component_marker] = 1
self.current_task = Task.FIND_NEXT_COMPONENT
self.task_history.append(self.current_task)
self.blocks_per_attachment.append(
self.current_blocks_per_attachment)
self.current_blocks_per_attachment = 0
self.steps_per_attachment.append(
self.current_steps_per_attachment)
self.current_steps_per_attachment = 0
self.current_path = None
self.current_visited_sites = None
return
# adding location and direction here to check for revisiting
if self.current_row_started:
self.current_visited_sites.append(current_site_tuple)
# the checks need to determine whether the current position is a valid attachment site
position_ahead_occupied = environment.check_occupancy_map(
self.current_grid_position + self.current_grid_direction)
position_ahead_to_be_empty = check_map(
self.target_map,
self.current_grid_position + self.current_grid_direction,
lambda x: x == 0)
position_around_corner_empty = environment.check_occupancy_map(
self.current_grid_position + self.current_grid_direction +
np.array([
-self.current_grid_direction[1],
self.current_grid_direction[0], 0
],
dtype="int64"), lambda x: x == 0)
row_ending = self.current_row_started and (
position_ahead_to_be_empty or position_around_corner_empty)
if loop_corner_attachable and allowable_region_attachable and \
check_map(self.target_map, self.current_grid_position) and \
(position_ahead_occupied or row_ending):
if ((environment.check_occupancy_map(self.current_grid_position + np.array([1, 0, 0])) and
environment.check_occupancy_map(self.current_grid_position + np.array([-1, 0, 0]))) or
(environment.check_occupancy_map(self.current_grid_position + np.array([0, 1, 0])) and
environment.check_occupancy_map(self.current_grid_position + np.array([0, -1, 0])))) and \
not environment.check_occupancy_map(self.current_grid_position, lambda x: x > 0):
# check if position is surrounded from two opposing sites
self.current_task = Task.LAND
self.current_visited_sites = None
self.current_path = None
self.aprint(
"CASE 1-3: Attachment site found, but block cannot be placed at {}."
.format(self.current_grid_position))
else:
# site should be occupied AND
# 1. site ahead has a block (inner corner) OR
# 2. the current "identified" row ends (i.e. no chance of obstructing oneself)
self.current_task = Task.PLACE_BLOCK
self.task_history.append(self.current_task)
self.current_visited_sites = None
self.current_row_started = False
self.current_path = None
log_string = "CASE 1-{}: Attachment site found, block can be placed at {}."
if environment.check_occupancy_map(
self.current_grid_position +
self.current_grid_direction):
log_string = log_string.format(
1, self.current_grid_position)
else:
log_string = log_string.format(
2, self.current_grid_position)
self.aprint(log_string)
sites = legal_attachment_sites(
self.target_map[self.current_structure_level],
environment.occupancy_map[
self.current_structure_level],
component_marker=self.current_component_marker)
self.per_search_attachment_site_count[
"possible"].append(1)
self.per_search_attachment_site_count["total"].append(
int(np.count_nonzero(sites)))
else:
# site should not be occupied -> determine whether to turn a corner or continue, options:
# 1. turn right (site ahead occupied)
# 2. turn left
# 3. continue straight ahead along perimeter
if position_ahead_occupied:
# turn right
self.current_grid_direction = np.array([
self.current_grid_direction[1],
-self.current_grid_direction[0], 0
],
dtype="int32")
self.aprint(
"CASE 2: Position straight ahead occupied, turning clockwise."
)
elif position_around_corner_empty:
# first move forward (to the corner)
self.current_path.add_position(
self.geometry.position +
Block.SIZE * self.current_grid_direction)
reference_position = self.current_path.positions[-1]
# then turn left
self.current_grid_position += self.current_grid_direction
self.current_grid_direction = np.array([
-self.current_grid_direction[1],
self.current_grid_direction[0], 0
],
dtype="int32")
self.current_grid_position += self.current_grid_direction
self.aprint(
"CASE 3: Reached corner of structure, turning counter-clockwise. {} {}"
.format(self.current_grid_position,
self.current_grid_direction))
self.current_path.add_position(
reference_position +
Block.SIZE * self.current_grid_direction)
self.current_row_started = True
else:
# otherwise site "around the corner" occupied -> continue straight ahead
self.current_grid_position += self.current_grid_direction
self.aprint(
"CASE 4: Adjacent positions ahead occupied, continuing to follow perimeter."
)
self.current_path.add_position(
self.geometry.position +
Block.SIZE * self.current_grid_direction)
self.current_row_started = True
if self.check_component_finished(self.local_occupancy_map):
self.current_task = Task.FIND_NEXT_COMPONENT
self.task_history.append(self.current_task)
self.current_visited_sites = None
self.current_path = None
else:
self.geometry.position = self.geometry.position + current_direction
self.per_task_distance_travelled[
Task.FIND_ATTACHMENT_SITE] += simple_distance(
position_before, self.geometry.position)
def find_attachment_site(self, environment: env.map.Map):
"""
Move with the goal of finding an attachment site.
This method is called if the current task is FIND_ATTACHMENT_SITE. If the agent has not planned a path yet,
it first determines all possible attachment sites in the current component, chooses one according to some
strategy and then plans a path to move there following the grid structure (not following the grid and still
counting blocks to maintain information about its position may be faster and may be feasible in a more
realistic simulation as well). Since this is the global knowledge version of the algorithm, all sites to be
occupied which do not currently violate the row rule are legal attachment sites. Unless the agent finds the
planned attachment site to be occupied upon arrival, the task changes to PLACE_BLOCK when that site is reached.
:param environment: the environment the agent operates in
"""
position_before = np.copy(self.geometry.position)
if self.current_path is None or self.current_shortest_path is None:
# get all legal attachment sites for the current component given the current occupancy matrix
attachment_sites = legal_attachment_sites(
self.component_target_map[self.current_structure_level],
environment.occupancy_map[self.current_structure_level],
component_marker=self.current_component_marker)
attachment_map = np.copy(attachment_sites)
# copy occupancy matrix to safely insert attachment sites
occupancy_map_copy = np.copy(
environment.occupancy_map[self.current_structure_level])
# convert to coordinates
attachment_sites = np.where(attachment_sites == 1)
attachment_sites = list(
zip(attachment_sites[1], attachment_sites[0]))
self.per_search_attachment_site_count["total"].append(
len(attachment_sites))
# remove all attachment sites which are not legal because of hole restrictions
backup = []
for site in attachment_sites:
at_loop_corner, loop_corner_attachable = self.check_loop_corner(
environment,
np.array([site[0], site[1], self.current_structure_level]))
allowable_region_attachable = True
if not at_loop_corner:
closing_corners = self.closing_corners[
self.current_structure_level][
self.current_component_marker]
for i in range(len(closing_corners)):
x, y, z = closing_corners[i]
orientation = self.closing_corner_orientations[
self.current_structure_level][
self.current_component_marker][i]
if not environment.check_occupancy_map(
np.array([x, y, z])):
if orientation == "NW":
if x >= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "NE":
if x <= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "SW":
if x >= site[0] and y >= site[1]:
allowable_region_attachable = False
break
elif orientation == "SE":
if x <= site[0] and y >= site[1]:
allowable_region_attachable = False
break
if loop_corner_attachable and allowable_region_attachable:
backup.append(site)
attachment_sites = backup
self.per_search_attachment_site_count["possible"].append(
len(attachment_sites))
# determine shortest paths to all attachment sites
shortest_paths = []
for x, y in attachment_sites:
occupancy_map_copy[y, x] = 1
sp = shortest_path(occupancy_map_copy,
(self.current_grid_position[0],
self.current_grid_position[1]), (x, y))
occupancy_map_copy[y, x] = 0
shortest_paths.append(sp)
# this should be used later:
directions = [
np.array([
x - self.current_grid_position[0],
y - self.current_grid_position[1]
]) for x, y in attachment_sites
]
counts = self.count_in_direction(environment,
directions=directions,
angle=np.pi / 2)
if self.attachment_site_ordering == "shortest_path":
if self.order_only_one_metric:
order = sorted(range(len(shortest_paths)),
key=lambda i:
(len(shortest_paths[i]), random.random()))
else:
order = sorted(range(len(shortest_paths)),
key=lambda i:
(len(shortest_paths[i]), counts[i]))
elif self.attachment_site_ordering == "agent_count":
if self.order_only_one_metric:
order = sorted(range(len(shortest_paths)),
key=lambda i: (counts[i], random.random()))
else:
order = sorted(range(len(shortest_paths)),
key=lambda i:
(counts[i], len(shortest_paths[i])))
else:
order = sorted(range(len(shortest_paths)),
key=lambda i: random.random())
shortest_paths = [shortest_paths[i] for i in order]
attachment_sites = [attachment_sites[i] for i in order]
# find "bends" in the path and only require going there
sp = shortest_paths[0]
if len(sp) > 1:
diffs = [(sp[1][0] - sp[0][0], sp[1][1] - sp[0][1])]
new_sp = [sp[0]]
for i in range(1, len(sp)):
if i < len(sp) - 1:
diff = (sp[i + 1][0] - sp[i][0],
sp[i + 1][1] - sp[i][1])
if diff[0] != diffs[-1][0] or diff[1] != diffs[-1][1]:
# a "bend" is happening
new_sp.append((sp[i]))
diffs.append(diff)
new_sp.append(sp[-1])
sp = new_sp
if not all(sp[-1][i] == attachment_sites[0][i] for i in range(2)):
# these are problems with a bug that I unfortunately did not have more time to investigate
# I took the practical approach of "solving" the problem by redoing the search for attachment
# sites here, which allows construction to continue, if nothing else
self.aprint(
"SHORTEST PATH DOESN'T LEAD TO INTENDED ATTACHMENT SITE",
override_global_printing_enabled=True)
self.aprint("Current grid position: {}".format(
self.current_grid_position),
override_global_printing_enabled=True)
self.aprint("Shortest path: {}".format(sp),
override_global_printing_enabled=True)
self.aprint("Intended attachment site: {}".format(
attachment_sites[0]),
override_global_printing_enabled=True)
self.aprint("Current component marker: {}".format(
self.current_component_marker),
override_global_printing_enabled=True)
self.aprint(
"Current seed at {} and seed's component marker: {}".
format(
self.current_seed.grid_position,
self.component_target_map[
self.current_seed.grid_position[2],
self.current_seed.grid_position[1],
self.current_seed.grid_position[0]]),
override_global_printing_enabled=True)
self.aprint("Attachment map:",
override_global_printing_enabled=True)
self.aprint(attachment_map,
print_as_map=True,
override_global_printing_enabled=True)
if check_map(self.component_target_map, self.current_seed.grid_position,
lambda x: x != self.current_component_marker) \
or check_map(self.component_target_map, self.current_grid_position,
lambda x: x != self.current_component_marker):
previous = self.current_seed
self.current_seed = environment.block_at_position(
self.component_seed_location(
self.current_component_marker))
if self.current_seed is None:
self.current_seed = previous
self.current_component_marker = self.component_target_map[
self.current_seed.grid_position[2],
self.current_seed.grid_position[1],
self.current_seed.grid_position[0]]
self.aprint("Setting new seed to {} with marker {}".format(
self.current_seed.grid_position,
self.component_target_map[
self.current_seed.grid_position[2],
self.current_seed.grid_position[1],
self.current_seed.grid_position[0]]),
override_global_printing_enabled=True)
self.aprint("Setting new marker to {}".format(
self.current_component_marker),
override_global_printing_enabled=True)
self.current_task = Task.TRANSPORT_BLOCK
self.transport_block(environment)
return
else:
raise Exception
# construct the path to that site
self.current_grid_position = np.array(
[sp[0][0], sp[0][1], self.current_structure_level])
self.current_path = Path()
position = [
environment.offset_origin[0] +
Block.SIZE * self.current_grid_position[0],
environment.offset_origin[0] +
Block.SIZE * self.current_grid_position[1],
self.geometry.position[2]
]
self.current_path.add_position(position)
# storing information about the path
self.current_shortest_path = sp
self.current_sp_index = 0
self.current_sp_search_count += 1
# check again whether attachment site is still available/legal
attachment_sites = legal_attachment_sites(
self.component_target_map[self.current_structure_level],
environment.occupancy_map[self.current_structure_level],
component_marker=self.current_component_marker)
if attachment_sites[self.current_shortest_path[-1][1],
self.current_shortest_path[-1][0]] == 0:
self.current_path = None
self.find_attachment_site(environment)
return
next_position, current_direction = self.move(environment)
if simple_distance(self.geometry.position,
next_position) <= Agent.MOVEMENT_PER_STEP:
self.geometry.position = next_position
ret = self.current_path.advance()
if not ret:
# have reached next point on shortest path to attachment site
current_spc = self.current_shortest_path[self.current_sp_index]
self.current_grid_position = np.array([
current_spc[0], current_spc[1],
self.current_structure_level
])
if self.current_sp_index >= len(
self.current_shortest_path) - 1:
# if the attachment site was determined definitively (corner or protruding), have reached
# intended attachment site and should assess whether block can be placed or not
if not environment.check_occupancy_map(
self.current_grid_position):
# if yes, just place it
self.current_task = Task.PLACE_BLOCK
self.task_history.append(self.current_task)
self.current_path = None
self.current_shortest_path = None
else:
# if no, need to find new attachment site (might just be able to restart this method)
self.current_path = None
self.current_shortest_path = None
else:
# if the attachment site was determined definitively (corner or protruding), have reached
# intended attachment site and should assess whether block can be placed or not
if environment.check_occupancy_map(
np.array([
self.current_shortest_path[-1][0],
self.current_shortest_path[-1][1],
self.current_grid_position[2]
])):
self.current_path = None
self.current_shortest_path = None
return
# still have to go on, therefore update the current path
self.current_sp_index += 1
next_spc = self.current_shortest_path[
self.current_sp_index]
next_position = [
environment.offset_origin[0] +
Block.SIZE * next_spc[0],
environment.offset_origin[0] +
Block.SIZE * next_spc[1], self.geometry.position[2]
]
self.current_path.add_position(next_position)
if self.check_component_finished(environment.occupancy_map):
self.recheck_task(environment)
self.task_history.append(self.current_task)
self.current_visited_sites = None
self.current_path = None
else:
# update local occupancy map while moving over the structure
block_below = environment.block_below(self.geometry.position)
# also need to check whether block is in shortest path
if block_below is not None and block_below.grid_position[2] == self.current_grid_position[2] \
and self.component_target_map[block_below.grid_position[2],
block_below.grid_position[1],
block_below.grid_position[0]] == self.current_component_marker:
# the following might not be enough, might have to check whether block was in the original SP
if self.current_sp_index < len(self.current_shortest_path) - 1:
self.current_grid_position = np.copy(
block_below.grid_position)
self.geometry.position = self.geometry.position + current_direction
self.per_task_distance_travelled[
Task.FIND_ATTACHMENT_SITE] += simple_distance(
position_before, self.geometry.position)
def find_attachment_site(self, environment: env.map.Map):
"""
Move with the goal of finding an attachment site.
This method is called if the current task is FIND_ATTACHMENT_SITE. If the agent has not planned a path yet,
it first determines all possible attachment sites in the current component, chooses one according to some
strategy and then plans a path to move there following the grid structure (not following the grid and still
counting blocks to maintain information about its position may be faster and may be feasible in a more
realistic simulation as well). Unless the agent finds the planned attachment site to be occupied upon arrival,
the task changes to PLACE_BLOCK when that site is reached.
:param environment: the environment the agent operates in
"""
position_before = np.copy(self.geometry.position)
if self.current_path is None or self.current_shortest_path is None:
self.update_local_occupancy_map(environment)
if self.check_component_finished(self.local_occupancy_map,
self.current_component_marker):
self.sp_search_count.append(
(self.current_sp_search_count,
int(self.current_component_marker),
self.current_task.name))
self.current_sp_search_count = 0
self.current_task = Task.FIND_NEXT_COMPONENT
self.find_next_component(environment)
return
# get all legal attachment sites for the current component given the current occupancy matrix
attachment_sites, corner_sites, protruding_sites, most_ccw_sites = \
legal_attachment_sites(self.component_target_map[self.current_structure_level],
self.local_occupancy_map[self.current_structure_level],
component_marker=self.current_component_marker, local_info=True)
sites = legal_attachment_sites(
self.target_map[self.current_structure_level],
environment.occupancy_map[self.current_structure_level],
component_marker=self.current_component_marker)
self.per_search_attachment_site_count["total"].append(
int(np.count_nonzero(sites)))
# copy occupancy matrix to safely insert attachment sites
occupancy_map_copy = np.copy(
self.local_occupancy_map[self.current_structure_level])
# remove all sites which are not allowed because of hole restrictions
backup = []
for site in corner_sites:
at_loop_corner, loop_corner_attachable = self.check_loop_corner(
environment,
np.array([site[0], site[1], self.current_structure_level]))
allowable_region_attachable = True
if not at_loop_corner:
closing_corners = self.closing_corners[
self.current_structure_level][
self.current_component_marker]
for i in range(len(closing_corners)):
x, y, z = closing_corners[i]
orientation = self.closing_corner_orientations[
self.current_structure_level][
self.current_component_marker][i]
if not environment.check_occupancy_map(
np.array([x, y, z])):
if orientation == "NW":
if x >= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "NE":
if x <= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "SW":
if x >= site[0] and y >= site[1]:
allowable_region_attachable = False
break
elif orientation == "SE":
if x <= site[0] and y >= site[1]:
allowable_region_attachable = False
break
if loop_corner_attachable and allowable_region_attachable:
backup.append(site)
corner_sites = backup
backup = []
for site in protruding_sites:
at_loop_corner, loop_corner_attachable = self.check_loop_corner(
environment,
np.array([site[0], site[1], self.current_structure_level]))
allowable_region_attachable = True
if not at_loop_corner:
closing_corners = self.closing_corners[
self.current_structure_level][
self.current_component_marker]
for i in range(len(closing_corners)):
x, y, z = closing_corners[i]
orientation = self.closing_corner_orientations[
self.current_structure_level][
self.current_component_marker][i]
if not environment.check_occupancy_map(
np.array([x, y, z])):
if orientation == "NW":
if x >= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "NE":
if x <= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "SW":
if x >= site[0] and y >= site[1]:
allowable_region_attachable = False
break
elif orientation == "SE":
if x <= site[0] and y >= site[1]:
allowable_region_attachable = False
break
# NOTE: actually not sure if the allowable region even plays into this because by their very nature
# these sites could pretty much only be corners themselves and not obstruct anything (?)
if loop_corner_attachable and allowable_region_attachable:
backup.append(site)
protruding_sites = backup
backup = []
for site, direction, expected_length in most_ccw_sites:
at_loop_corner, loop_corner_attachable = self.check_loop_corner(
environment,
np.array([site[0], site[1], self.current_structure_level]))
allowable_region_attachable = True
if not at_loop_corner:
closing_corners = self.closing_corners[
self.current_structure_level][
self.current_component_marker]
for i in range(len(closing_corners)):
x, y, z = closing_corners[i]
orientation = self.closing_corner_orientations[
self.current_structure_level][
self.current_component_marker][i]
if not environment.check_occupancy_map(
np.array([x, y, z])):
if orientation == "NW":
if x >= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "NE":
if x <= site[0] and y <= site[1]:
allowable_region_attachable = False
break
elif orientation == "SW":
if x >= site[0] and y >= site[1]:
allowable_region_attachable = False
break
elif orientation == "SE":
if x <= site[0] and y >= site[1]:
allowable_region_attachable = False
break
# in this case, might have to block attachment if row reaches into either of these
# regions, but I'm not sure that this can happen
if loop_corner_attachable and allowable_region_attachable:
backup.append((site, direction, expected_length))
most_ccw_sites = backup
if self.attachment_site_order == "prioritise":
# find the closest corner or protruding site
# if there are none then take the closest most CCW site
if len(corner_sites) != 0:
attachment_sites = [(s, ) for s in corner_sites]
elif len(protruding_sites) != 0:
attachment_sites = [(s, ) for s in protruding_sites]
else:
attachment_sites = most_ccw_sites
elif self.attachment_site_order in [
"shortest_path", "shortest_travel_path", "agent_count"
]:
attachment_sites = []
attachment_sites.extend([(s, ) for s in corner_sites])
attachment_sites.extend([(s, ) for s in protruding_sites])
attachment_sites.extend(most_ccw_sites)
self.per_search_attachment_site_count["possible"].append(
len(attachment_sites))
# determine shortest paths to all attachment sites
shortest_paths = []
for tpl in attachment_sites:
x, y = tpl[0]
occupancy_map_copy[y, x] = 1
sp = shortest_path(occupancy_map_copy,
(self.current_grid_position[0],
self.current_grid_position[1]), (x, y))
occupancy_map_copy[y, x] = 0
shortest_paths.append(sp)
if self.attachment_site_order in ["prioritise", "shortest_path"] \
or self.attachment_site_order == "shortest_travel_path" and len(most_ccw_sites) == 0:
sorted_indices = sorted(range(len(attachment_sites)),
key=lambda i: len(shortest_paths[i]))
elif self.attachment_site_order == "shortest_travel_path":
sorted_indices = sorted(range(len(attachment_sites)),
key=lambda i: len(shortest_paths[i]) +
attachment_sites[i][2]
if len(attachment_sites[i]) > 1 else 0)
elif self.attachment_site_order == "agent_count":
directions = [
np.array([
s[0][0] - self.current_grid_position[0],
s[0][1] - self.current_grid_position[1]
]) for s in attachment_sites
]
counts = self.count_in_direction(environment,
directions=directions,
angle=np.pi / 2)
if self.order_only_one_metric:
sorted_indices = sorted(range(len(attachment_sites)),
key=lambda i:
(counts[i], random.random()))
else:
sorted_indices = sorted(
range(len(attachment_sites)),
key=lambda i: (counts[i], len(shortest_paths[i])))
else:
sorted_indices = sorted(range(len(attachment_sites)),
key=lambda i: random.random())
attachment_sites = [attachment_sites[i] for i in sorted_indices]
shortest_paths = [shortest_paths[i] for i in sorted_indices]
sp = shortest_paths[0]
# find the "bends" in the path and remove all sites in-between which are not really required
if len(sp) > 1:
diffs = [(sp[1][0] - sp[0][0], sp[1][1] - sp[0][1])]
new_sp = [sp[0]]
for i in range(1, len(sp)):
if i < len(sp) - 1:
diff = (sp[i + 1][0] - sp[i][0],
sp[i + 1][1] - sp[i][1])
if diff[0] != diffs[-1][0] or diff[1] != diffs[-1][1]:
# a "bend" is happening
new_sp.append((sp[i]))
diffs.append(diff)
new_sp.append(sp[-1])
sp = new_sp
# if the CCW sites are used, then store the additional required information
# also need to make sure to reset this to None because it will be used for checks
self.current_attachment_info = None
if len(attachment_sites[0]) > 1:
self.current_attachment_info = attachment_sites[0]
# construct the path to that site
self.current_grid_position = np.array(
[sp[0][0], sp[0][1], self.current_structure_level])
self.current_path = Path()
position = [
environment.offset_origin[0] +
Block.SIZE * self.current_grid_position[0],
environment.offset_origin[0] +
Block.SIZE * self.current_grid_position[1],
self.geometry.position[2]
]
self.current_path.add_position(position)
# storing information about the path
self.current_shortest_path = sp
self.current_sp_index = 0
self.current_sp_search_count += 1
next_position, current_direction = self.move(environment)
if simple_distance(self.geometry.position,
next_position) <= Agent.MOVEMENT_PER_STEP:
self.geometry.position = next_position
ret = self.current_path.advance()
if not ret:
block_below = environment.block_below(self.geometry.position)
if block_below is not None and block_below.grid_position[
2] > self.current_structure_level:
# there is a block below that is higher than the current layer
# since layers are built sequentially this means that the current layer must be completed
# therefore, this is noted in the local map and we move up to the layer of that block
# and start looking for some other component
for layer in range(block_below.grid_position[2]):
self.local_occupancy_map[layer][
self.target_map[layer] != 0] = 1
self.current_structure_level = block_below.grid_position[2]
self.sp_search_count.append(
(self.current_sp_search_count,
int(self.current_component_marker),
self.current_task.name))
self.current_sp_search_count = 0
self.current_task = Task.FIND_NEXT_COMPONENT
self.task_history.append(self.current_task)
self.current_grid_position = block_below.grid_position
self.current_path = None
return
# have reached next point on shortest path to attachment site
if self.current_attachment_info is not None and self.current_sp_index >= len(
self.current_shortest_path):
self.current_grid_position = self.current_grid_position + self.current_grid_direction
else:
current_spc = self.current_shortest_path[
self.current_sp_index]
self.current_grid_position = np.array([
current_spc[0], current_spc[1],
self.current_structure_level
])
self.update_local_occupancy_map(environment)
if self.current_sp_index >= len(
self.current_shortest_path) - 1:
# at the end of the shortest path to the attachment site (if the site was an end-of-row/CCW site,
# then still need to continue going down that row)
if self.current_attachment_info is None:
# if the attachment site was determined definitively (corner or protruding), have reached
# intended attachment site and should assess whether block can be placed or not
if not environment.check_occupancy_map(
self.current_grid_position):
# if yes, just place it
self.current_task = Task.PLACE_BLOCK
self.task_history.append(self.current_task)
self.current_path = None
self.current_shortest_path = None
else:
# if no, need to find new attachment site (might just be able to restart this method)
self.current_path = None
self.current_shortest_path = None
else:
# otherwise, if at starting position for the row/column/perimeter search, need to check
# whether attachment is already possible and if not, plan next path movement
# note that due to the code above, the local map should already be updated, making it
# possible to decide whether we are at an outer corner globally
self.current_sp_index += 1
self.current_grid_direction = self.current_attachment_info[
1]
if environment.check_occupancy_map(
self.current_grid_position):
# if there is a block at the position, it means that we are still at the end of the
# shortest path and all the sites should have been filled already, meaning that the
# local occupancy matrix for that row/column can be filled out as well
position = self.current_grid_position.copy(
) + self.current_grid_direction
# could either do this according to local occupancy map or (if that results in problems)
# with checking component completeness, could "physically" explore this row to be sure
# that it is finished/to update the local occupancy map up until the next intended gap
while 0 <= position[0] < self.target_map.shape[2] \
and 0 <= position[1] < self.target_map.shape[1] \
and check_map(self.target_map, position) \
and check_map(self.local_occupancy_map, position):
self.local_occupancy_map[position[2],
position[1],
position[0]] = 1
position = position + self.current_grid_direction
# afterwards, continue search for an attachment site
self.current_path = None
else:
# check whether attachment possible right now
position_ahead_occupied = environment.check_occupancy_map(
self.current_grid_position +
self.current_grid_direction)
position_ahead_to_be_empty = check_map(
self.target_map, self.current_grid_position +
self.current_grid_direction, lambda x: x == 0)
position_around_corner_empty = environment.check_occupancy_map(
self.current_grid_position +
self.current_grid_direction +
np.array([
-self.current_grid_direction[1],
self.current_grid_direction[0], 0
],
dtype="int64"), lambda x: x == 0)
if position_ahead_occupied or position_around_corner_empty or position_ahead_to_be_empty:
# attachment possible
self.current_task = Task.PLACE_BLOCK
self.task_history.append(self.current_task)
self.current_path = None
self.current_shortest_path = None
self.current_attachment_info = None
else:
# cannot place block yet, move on to next location
next_grid_position = self.current_grid_position + self.current_grid_direction
next_position = [
environment.offset_origin[0] +
Block.SIZE * next_grid_position[0],
environment.offset_origin[0] +
Block.SIZE * next_grid_position[1],
self.geometry.position[2]
]
self.current_path.add_position(next_position)
else:
if self.current_attachment_info is None:
# if the attachment site was determined definitively (corner or protruding), have reached
# intended attachment site and should assess whether block can be placed or not
if check_map(
self.local_occupancy_map,
np.array([
self.current_shortest_path[-1][0],
self.current_shortest_path[-1][1],
self.current_grid_position[2]
])):
# if no, need to find new attachment site (might just be able to restart this method)
self.current_path = None
self.current_shortest_path = None
return
# still have to go on, therefore update the current path
self.current_sp_index += 1
next_spc = self.current_shortest_path[
self.current_sp_index]
next_position = [
environment.offset_origin[0] +
Block.SIZE * next_spc[0],
environment.offset_origin[0] +
Block.SIZE * next_spc[1], self.geometry.position[2]
]
self.current_path.add_position(next_position)
if self.check_component_finished(self.local_occupancy_map):
self.current_task = Task.FIND_NEXT_COMPONENT
self.task_history.append(self.current_task)
self.current_visited_sites = None
self.current_path = None
self.sp_search_count.append(
(self.current_sp_search_count,
int(self.current_component_marker),
self.current_task.name))
self.current_sp_search_count = 0
else:
# update local occupancy map while moving over the structure
block_below = environment.block_below(self.geometry.position)
# also need to check whether block is in shortest path
if block_below is not None and block_below.grid_position[2] == self.current_grid_position[2] \
and self.component_target_map[block_below.grid_position[2],
block_below.grid_position[1],
block_below.grid_position[0]] == self.current_component_marker:
# the following might not be enough, might have to check whether block was in the original SP
if self.current_sp_index < len(self.current_shortest_path) - 1:
self.current_grid_position = np.copy(
block_below.grid_position)
self.update_local_occupancy_map(environment)
self.geometry.position = self.geometry.position + current_direction
self.per_task_distance_travelled[
Task.FIND_ATTACHMENT_SITE] += simple_distance(
position_before, self.geometry.position)