#
# This is a definition of a maze environment simulation engine. It provides
# routines to read maze configuration and build related simulation environment
# from it. Also it provides method to simulate the behavior of the navigating agent
# and interaction with his sensors.
#
import math
import agent
import geometry
from scipy.spatial import distance
from novelty_archive import NoveltyItem
# The maximal allowed speed for the maze solver agent
MAX_AGENT_SPEED = 3.0
def maze_novelty_metric(first_item, second_item):
"""
The function to calculate the novelty metric score as a distance between two
data vectors in provided NoveltyItems
Arguments:
first_item: The first NoveltyItem
second_item: The second NoveltyItem
Returns:
The novelty metric as a distance between two
data vectors in provided NoveltyItems
"""
if not (hasattr(first_item, "data") or hasattr(second_item, "data")):
return NotImplemented
if len(first_item.data) != len(second_item.data):
# can not be compared
return 0.0
diff_accum = 0.0
size = len(first_item.data)
for i in range(size):
diff = abs(first_item.data[i] - second_item.data[i])
diff_accum += diff
return diff_accum / float(size)
def maze_novelty_metric_euclidean(first_item, second_item):
"""
The function to calculate the novelty metric score as a distance between two
data vectors in provided NoveltyItems
Arguments:
first_item: The first NoveltyItem
second_item: The second NoveltyItem
Returns:
The novelty metric as a distance between two
data vectors in provided NoveltyItems
"""
if not (hasattr(first_item, "data") or hasattr(second_item, "data")):
return NotImplemented
if len(first_item.data) != len(second_item.data):
# can not be compared
return 0.0
return distance.euclidean(first_item.data, second_item.data)
class MazeEnvironment:
"""
This class encapsulates the maze simulation environment.
"""
def __init__(self, agent, walls, exit_point, exit_range=5.0):
"""
Creates new maze environment with specified walls and exit point.
Arguments:
agent: The maze navigating agent
walls: The maze walls
exit_point: The maze exit point
exit_range: The range arround exit point marking exit area
"""
self.walls = walls
self.exit_point = exit_point
self.exit_range = exit_range
# The maze navigating agent
self.agent = agent
# The flag to indicate if exit was found
self.exit_found = False
# The initial distance of agent from exit
self.initial_distance = self.agent_distance_to_exit()
# Update sensors
self.update_rangefinder_sensors()
self.update_radars()
def agent_distance_to_exit(self):
"""
The function to estimate distance from maze solver agent to the maze exit.
Returns:
The distance from maze solver agent to the maze exit.
"""
return self.agent.location.distance(self.exit_point)
def test_wall_collision(self, loc):
"""
The function to test if agent at specified location collides with any
of the maze walls.
Arguments:
loc: The new agent location to test for collision.
Returns:
The True if agent at new location will collide with any of the maze walls.
"""
for w in self.walls:
if w.distance(loc) < self.agent.radius:
return True
return False
def create_net_inputs(self):
"""
The function to create the ANN input values from the simulation environment.
Returns:
The list of ANN inputs consist of values get from solver agent sensors.
"""
inputs = []
# The range finders
for ri in self.agent.range_finders:
inputs.append(ri)
# The radar sensors
for rs in self.agent.radar:
inputs.append(rs)
return inputs
def apply_control_signals(self, control_signals):
"""
The function to apply control signals received from control ANN to the
maze solver agent.
Arguments:
control_signals: The control signals received from the control ANN
"""
self.agent.angular_vel += (control_signals[0] - 0.5)
self.agent.speed += (control_signals[1] - 0.5)
# constrain the speed & angular velocity
if self.agent.speed > MAX_AGENT_SPEED:
self.agent.speed = MAX_AGENT_SPEED
if self.agent.speed < -MAX_AGENT_SPEED:
self.agent.speed = -MAX_AGENT_SPEED
if self.agent.angular_vel > MAX_AGENT_SPEED:
self.agent.angular_vel = MAX_AGENT_SPEED
if self.agent.angular_vel < -MAX_AGENT_SPEED:
self.agent.angular_vel = -MAX_AGENT_SPEED
def update_rangefinder_sensors(self):
"""
The function to update the agent range finder sensors.
"""
for i, angle in enumerate(self.agent.range_finder_angles):
rad = geometry.deg_to_rad(angle)
# project a point from agent location outwards
projection_point = geometry.Point(
x = self.agent.location.x + math.cos(rad) * self.agent.range_finder_range,
y = self.agent.location.y + math.sin(rad) * self.agent.range_finder_range
)
# rotate the projection point by the agent's heading angle to
# align it with heading direction
projection_point.rotate(self.agent.heading, self.agent.location)
# create the line segment from the agent location to the projected point
projection_line = geometry.Line(
a = self.agent.location,
b = projection_point
)
# set range to maximum detection range
min_range = self.agent.range_finder_range
# now test against maze walls to see if projection line hits any wall
# and find the closest hit
for wall in self.walls:
found, intersection = wall.intersection(projection_line)
if found:
found_range = intersection.distance(self.agent.location)
# we are interested in the closest hit
if found_range < min_range:
min_range = found_range
# Update sensor value
self.agent.range_finders[i] = min_range
def update_radars(self):
"""
The function to update the agent radar sensors.
"""
target = geometry.Point(self.exit_point.x, self.exit_point.y)
# rotate target with respect to the agent's heading to align it with heading direction
target.rotate(self.agent.heading, self.agent.location)
# translate with respect to the agent's location
target.x -= self.agent.location.x
target.y -= self.agent.location.y
# the angle between maze exit point and the agent's heading direction
angle = target.angle()
# find the appropriate radar sensor to be fired
for i, r_angles in enumerate(self.agent.radar_angles):
self.agent.radar[i] = 0.0 # reset specific radar
if (angle >= r_angles[0] and angle < r_angles[1]) or (angle + 360 >= r_angles[0] and angle + 360 < r_angles[1]):
self.agent.radar[i] = 1.0 # fire the radar
def update(self, control_signals):
"""
The function to update solver agent position within maze. After agent position
updated it will be checked to find out if maze exit was reached afetr that.
Arguments:
control_signals: The control signals received from the control ANN
Returns:
The True if maze exit was found after update or maze exit was already
found in previous simulation cycles.
"""
if self.exit_found:
# Maze exit already found
return True
# Apply control signals
self.apply_control_signals(control_signals)
# get X and Y velocity components
vx = math.cos(geometry.deg_to_rad(self.agent.heading)) * self.agent.speed
vy = math.sin(geometry.deg_to_rad(self.agent.heading)) * self.agent.speed
# Update current Agent's heading (we consider the simulation time step size equal to 1s
# and the angular velocity as degrees per second)
self.agent.heading += self.agent.angular_vel
# Enforce angular velocity bounds by wrapping
if self.agent.heading > 360:
self.agent.heading -= 360
elif self.agent.heading < 0:
self.agent.heading += 360
# find the next location of the agent
new_loc = geometry.Point(
x = self.agent.location.x + vx,
y = self.agent.location.y + vy
)
if not self.test_wall_collision(new_loc):
self.agent.location = new_loc
# update agent's sensors
self.update_rangefinder_sensors()
self.update_radars()
# check if agent reached exit point
distance = self.agent_distance_to_exit()
self.exit_found = (distance < self.exit_range)
return self.exit_found
def __str__(self):
"""
Returns the nicely formatted string representation of this environment.
"""
str = "MAZE\nAgent at: (%.1f, %.1f)" % (self.agent.location.x, self.agent.location.y)
str += "\nExit at: (%.1f, %.1f), exit range: %.1f" % (self.exit_point.x, self.exit_point.y, self.exit_range)
str += "\nWalls [%d]" % len(self.walls)
for w in self.walls:
str += "\n\t%s" % w
return str
def read_environment(file_path):
"""
The function to read maze environment configuration from provided
file.
Arguments:
file_path: The path to the file to read maze configuration from.
Returns:
The initialized maze environment.
"""
num_lines, index = -1, 0
walls = []
maze_agent, maze_exit = None, None
with open(file_path, 'r') as file:
for line in file.readlines():
line = line.strip()
if len(line) == 0:
# skip empty lines
continue
if index == 0:
# read the number of line segments
num_lines = int(line)
elif index == 1:
# read the agent's position
loc = geometry.read_point(line)
maze_agent = agent.Agent(location=loc)
elif index == 2:
# read the agent's initial heading
maze_agent.heading = float(line)
elif index == 3:
# read the maze exit location
maze_exit = geometry.read_point(line)
else:
# read the walls
wall = geometry.read_line(line)
walls.append(wall)
# increment cursor
index += 1
assert len(walls) == num_lines
print("Maze environment configured successfully from the file: %s" % file_path)
# create and return the maze environment
return MazeEnvironment(agent=maze_agent, walls=walls, exit_point=maze_exit)
def maze_simulation_evaluate(env, net, time_steps, n_item=None, path_points=None):
"""
The function to evaluate maze simulation for specific environment
and controll ANN provided. The results will be saved into provided
agent record holder.
Arguments:
env: The maze configuration environment.
net: The maze solver agent's control ANN.
time_steps: The number of time steps for maze simulation.
n_item: The NoveltyItem to store evaluation results.
path_points: The holder for path points collected during simulation. If
provided None then nothing will be collected.
Returns:
The goal-oriented fitness value, i.e., how close is agent to the exit at
the end of simulation.
"""
exit_found = False
for i in range(time_steps):
if maze_simulation_step(env, net):
print("Maze solved in %d steps" % (i + 1))
exit_found = True
if path_points is not None:
# collect current position
path_points.append(geometry.Point(env.agent.location.x, env.agent.location.y))
if exit_found:
break
# store final agent coordinates as genome's novelty characteristics
if n_item is not None:
n_item.data.append(env.agent.location.x)
n_item.data.append(env.agent.location.y)
return env.agent_distance_to_exit()
def maze_simulation_step(env, net):
"""
The function to perform one step of maze simulation.
Arguments:
env: The maze configuration environment.
net: The maze solver agent's control ANN
Returns:
The True if maze agent solved the maze.
"""
# create inputs from the current state of the environment
inputs = env.create_net_inputs()
# load inputs into controll ANN and get results
output = net.activate(inputs)
# apply control signal to the environment and update
return env.update(output)
