def test_5x5_sense(self):
state_actual = self.env_5x5._sense_from_position(Position(0, 0))
state_actual = state_actual.asStd()
state_expected = {0: [0, -1], 1: [1, 1], 2: [1, 1], 3: [0, -1]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_5x5._sense_from_position(Position(0, 1))
state_actual = state_actual.asStd()
state_expected = {0: [0, -1], 1: [1, 1], 2: [1, 1], 3: [1, 0]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_5x5._sense_from_position(Position(0, 4))
state_actual = state_actual.asStd()
state_expected = {0: [0, -1], 1: [0, -1], 2: [1, 1], 3: [1, 1]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_5x5._sense_from_position(Position(4, 4))
state_actual = state_actual.asStd()
state_expected = {0: [1, 1], 1: [0, -1], 2: [0, -1], 3: [1, 1]}
self.assertEqual(state_actual, state_expected)
def __init__(self):
# initialize devices
self.initializeDevices()
# initial speed and steering angle values
self.speed = 0.0
self.angle = 0.0
# initial wheel length used to calculate how many m the wheels have traveled
self.leftWheelDistance = 0.0
self.rightWheelDistance = 0.0
self.updateDistanceTraveled()
# line forwared
self.lineFollower = LineFollower(self.camera)
# navigation
self.nav = PathPlanner()
self.nav.setRobotPosition(Position(4, 1))
self.nav.setGoalPosition(Position(14, 22))
self.nav.setRobotOrientation(SOUTH)
self.nav.printStatus()
# compute fastest route from robot to goal
self.turns = self.nav.getFastestRoute()
logger.debug("turns: " + str(self.turns))
# odometry
self.odometry = Odometry(self.positionSensors, self.compass,
self.nav.getRobotPosition(),
self.nav.getRobotOrientation())
def _make_action(self, rid, action):
"""
:param rid: id of the agent
:param action: action for the agent
:return: tuple of following:
state_prime: new state for the agent
reward: reward for the agent
done: agent terminated or not
info: anything else
"""
agent = self.agents[rid]
pos = agent.pos
# action: 0-north, 1-east, 2-south, 3-west
new_pos = Position(-1, -1)
if action == Direction.UP or action == Direction.UP.value:
new_pos = pos.up()
elif action == Direction.RIGHT or action == Direction.RIGHT.value:
new_pos = pos.right()
elif action == Direction.DOWN or action == Direction.DOWN.value:
new_pos = pos.down()
elif action == Direction.LEFT or action == Direction.LEFT.value:
new_pos = pos.left()
else:
print("Invalid direction: %s" % (action))
agent._set_pos(new_pos)
try:
assert new_pos >= Position(
0, 0), "out of bounds - outside map (below_min)"
assert new_pos < Position(
self.height,
self.width), "out of bounds - outside map (above_max)"
assert self.grid[
new_pos.asTuple()] != 0, "out of bounds - internal edge"
except Exception as e:
# print("position:", new_pos, "is", e)
# print("\tRemember (in y,x format) the grid size is", self.grid.shape)
self.dead = True
reward = self._get_reward(new_pos)
state_prime = agent._get_state(new_pos)
return StepResponse(state_prime, reward, True)
# return None, reward, True, None
state_prime = agent._get_state(new_pos)
reward = self._get_reward(new_pos)
done = self._get_terminate(new_pos)
resp = StepResponse(state_prime, reward, done)
return resp
def test_2x1_sense(self):
state_actual = self.env_2x1._sense_from_position(Position(0, 0))
state_actual = state_actual.asStd()
state_expected = {0: [0], 1: [1], 2: [0], 3: [0]}
self.assertEqual(state_actual, state_expected)
state_actual = self.env_2x1._sense_from_position(Position(0, 1))
state_actual = state_actual.asStd()
state_expected = {0: [0], 1: [0], 2: [0], 3: [1]}
self.assertEqual(state_actual, state_expected)
def updatePosition(self):
speed = self.actuators.getSpeed()
if speed != 0:
# get actual float map position
x = self.position.x
y = self.position.y
# 72 = 0.50 m/s * speed/MAX_SPEED [1.8] / 40 step/s / 0.5 m
# linearMove = ((0.50 * (speed/MAX_SPEED)) / 40) * 2
linearMove = speed/72
# compass decimal digits
precision = 2
turnCoeficent = 1
# if turning you need to do less meters in order to change position in the map
steeringAngle = self.actuators.getAngle()
if abs(steeringAngle) == 0.57:
turnCoeficent = 1.2
# update position
newX = x - round(self.compass.getXComponent(), precision) * linearMove * turnCoeficent
newY = y + round(self.compass.getYComponent(), precision) * linearMove * turnCoeficent
self.setPosition(Position(newX,newY))
def findNearestIntersection(position, radius = 1, orientation = False):
x = position.getX()
y = position.getY()
for i in range(x-radius, x+radius +1):
for j in range(y-radius, y+radius +1):
if i < HEIGHT and i > 0 and j < WIDTH and j > 0:
if MAP[i][j] == I:
return Position(i, j)
return -1
def setUp(self):
self.env_2x1 = Environment(width=2,
height=1,
num_agents=1,
start=Position(0, 0),
goal=Position(0, 1),
view_range=1)
self.env_5x5 = Environment(width=5,
height=5,
num_agents=1,
start=Position(0, 0),
goal=Position(4, 4),
view_range=2)
self.env_3x3_std = Environment(width=3,
height=3,
num_agents=1,
start=(0, 0),
goal=(2, 2),
view_range=1,
std=True)
def getNearestWalkablePosition(position, orientation):
if not isWalkable(position):
logger.debug("Actual position non walkable. " + str(position) + " is unwalkable")
x = position.getX()
y = position.getY()
logger.debug("ORIENTATION: " + str(orientation))
if orientation == Orientation.NORD or orientation == Orientation.SOUTH:
p = Position(x, y - 1)
if isWalkable(p):
return p
p = Position(x, y + 1)
if isWalkable(p):
return p
elif orientation == Orientation.EAST or orientation == Orientation.WEST:
p = Position(x - 1, y)
if isWalkable(p):
return p
p = Position(x + 1, y)
if isWalkable(p):
return p
else:
return position
def getNearestWalkablePosition2(position, orientation):
if not isWalkable(position):
x = position.getX()
y = position.getY()
radius = 1
for i in range(x-radius, x+radius +1):
for j in range(y-radius, y+radius +1):
if i < HEIGHT and i > 1 and j < WIDTH and j > 1:
p = Position(x+i, y+j)
if isWalkable(p):
return p
else:
return position
def getNearestWalkablePositionEquals(position, orientation, value):
if not isWalkable(position):
logger.debug("Actual position non walkable. " + str(position) + " is unwalkable")
x = position.getX()
y = position.getY()
if orientation == Orientation.NORD or orientation == Orientation.SOUTH:
p = Position(x+1, y)
if isWalkable(p) and getValue(p) == value:
return p
p = Position(x-1, y)
if isWalkable(p) and getValue(p) == value:
return p
elif orientation == Orientation.EAST or orientation == Orientation.WEST:
p = Position(x, y+1)
if isWalkable(p) and getValue(p) == value:
return p
p = Position(x, y-1)
if isWalkable(p) and getValue(p) == value:
return p
elif getValue(position) == value:
return position
else:
return -1
def updatePath(self):
if self.isEnabled():
logger.info("Computing new path..")
p = self.positioning.getPosition()
o = self.positioning.getOrientation()
nearest = Map.getNearestWalkablePosition(p, o)
if nearest != None:
p = nearest
x = p.getX()
y = p.getY()
if o == Orientation.NORD:
Map.setNewObstacle(Position(x - 1, y))
if o == Orientation.EAST:
Map.setNewObstacle(Position(x, y + 1))
if o == Orientation.SOUTH:
Map.setNewObstacle(Position(x + 1, y))
if o == Orientation.WEST:
Map.setNewObstacle(Position(x, y - 1))
if DEBUG:
Map.printMap()
self.currentPath = self.pathPlanner.getFastestRoute()
self.actualTurn = 0
def __init__(self, positionSensors, compass, lineFollower, actuators):
self.positionSensors = positionSensors
self.frontLeft = positionSensors.frontLeft
self.frontRight = positionSensors.frontRight
self.compass = compass
self.lineFollower = lineFollower
self.actuators = actuators
self.leftWheelDistance = 0.0
self.rightWheelDistance = 0.0
self.reference = self.getActualDistance()
self.lineAlreadyLost = False
self.position = Position(SPX,SPY)
self.orientation = UNKNOWN
self.inaccurateOrientation = UNKNOWN
self.updateOrientation()
def __init__(self,
height=5,
width=5,
num_agents=1,
start=Position(0, 0),
goal=Position(9E9, 9E9),
view_range=1,
horizon=2,
goal_reward=1.0,
pos_hist_len=5,
render=False,
std=False):
self.std = std
self.width = width
self.height = height
self.num_agents = num_agents
self.start = start
self.goal = goal
self.view_range = view_range
self.dead = False
self.pos_hist_len = pos_hist_len
if self.std:
start = Position(y=start[0], x=start[1])
goal = Position(y=goal[0], x=goal[1])
if goal.x > self.width or goal.y > self.height:
goal = Position(self.height - 1, self.width - 1)
# self.grid stores state of each grid of the map
# 0-obstacle, 1-walkable, 2-goal, 3-agent
self.grid = np.ones((height, width), dtype=int)
self.grid[goal.asTuple()] = 2
self.num_agents = num_agents
self.agents = {}
for n in range(self.num_agents):
self.agents[n] = Agent(self,
pos_hist_len=self.pos_hist_len,
pos_start=self.start)
self.reward_map = RewardMap(self.goal, self.height, self.width,
horizon, goal_reward)
# self.reward_map = np.zeros((height,width))
# self.reward_map[goal.asTuple()] = 1.0
self.reward_death = -100.0
self.renderEnv = render
self.called_render = False
if self.renderEnv:
self._render()
def __init__(self):
self.map = MAP
self.robotPosition = Position(0, 0)
self.robotOrientation = NORD
self.goalPosition = Position (0, 0)
def __init__(self, positioning):
self.map = Map.MAP
self.positioning = positioning
self.robotPosition = positioning.getPosition()
self.robotOrientation = positioning.getOrientation()
self.goalPosition = Position (14, 23)
def __init__(self, imageName):
self._pos = Position(1, 1)
self._isTransposable = True
self._isMortal = False
from Environment import Environment
from Utils import Agent, Direction, Position, StepResponse
import curses
import time
env = Environment(width=20,
height=20,
num_agents=1,
start=Position(0, 0),
goal=Position(19, 19),
view_range=2,
render=True)
def main(stdscr):
# do not wait for input when calling getch
stdscr.nodelay(1)
done = False
while not done:
# get keyboard input, returns -1 if none available
c = stdscr.getch()
if c >= 258 and c <= 261:
# print numeric value
if c == 258:
a = 2
elif c == 259:
a = 0
elif c == 260:
# self.im.set_data(grid_copy)
self.im.set_data(self.grid_copy)
self.fig.suptitle("Agent in grid world, plus edges")
plt.draw()
plt.show(block=block)
# if (iteration % 2 == 0 and iteration <= 8) or iteration == 40:
# fig.savefig("P=%.1f_t=%d_Iteration=%d.png" % (P, t, iteration))
plt.pause(0.001)
if __name__ == "__main__":
env = Environment(width=5,
height=5,
num_agents=1,
start=Position(0, 0),
goal=Position(4, 4),
view_range=3,
render=True)
print(env.get_state()[0].as1xnArray().shape[0])
"""
env.step returns a library of agent stepResponses,
each stepResponse.asTuple() is a tuple w/ (State Obj, reward, done, info)
each state object
"""
agents = env.step({0: Direction.RIGHT})
stepResponse = agents[0]
# print(agents[0].asTuple())