def driveToGoal(self):
if self.isCloseToOtherCar():
self.velocity = Vec2d(0, 0)
self.agentComm.unclaimIntersection(self)
return
#self.accelerate(Agent.ACCELERATION)
#return
frontOfCar = self.pos + self.dir.normalized() * Car.LENGTH
if self.inIntersection:
inter = self.model.getIntersection(frontOfCar.x, frontOfCar.y)
if not self.agentComm.claimIntersection(inter, self): return
self.accelerate(Agent.ACCELERATION)
if not inter:
self.agentComm.unclaimIntersection(self)
self.inIntersection = False
else:
if self.model.inIntersection(frontOfCar.x, frontOfCar.y):
self.velocity = Vec2d(0, 0)
self.agentComm.unclaimIntersection(self)
self.inIntersection = True
else:
self.accelerate(Agent.ACCELERATION)
def getNodePos(self, side, isIn, inIsLeft):
width = self.block.getWidth()
height = self.block.getHeight()
center = self.block.getCenter()
isLeft = inIsLeft != isIn
if isLeft:
offset = Vec2d(-width * .2, -height / 2.0)
else:
offset = Vec2d(width * .2, -height / 2.0)
angle = Intersection.ANGLE_FROM_NORTH[side]
offset.rotate(angle)
return center + offset
def accelerate(self, amount):
amount = min(amount, Car.MAX_ACCELERATION)
acceleration = Vec2d(self.dir.x, self.dir.y).normalized()
acceleration *= amount
self.velocity += acceleration
if (self.velocity.get_length() >= self.maxSpeed):
self.velocity.set_length(self.maxSpeed)
def move(self, cars):
for car in cars:
start = time.time()
oldDir = Vec2d(car.dir.x, car.dir.y)
oldPos = Vec2d(car.pos.x, car.pos.y)
car.update()
newPos = car.getPos()
newDir = car.getDir()
deltaPos = newPos - oldPos
deltaAngle = oldDir.get_angle_between(newDir)
self.updateTime += time.time() - start
if Const.SHOW_CARS or car.isJunior():
self.moveCarDisplay(car, deltaPos, deltaAngle)
if self.isLearning:
self.learner.noteCarMove(oldPos, newPos)
def autonomousAction(self, beliefs, agentGraph):
oldPos = Vec2d(self.pos.x, self.pos.y)
oldDir = Vec2d(self.dir.x, self.dir.y)
oldVel = Vec2d(self.velocity.x, self.velocity.y)
actions = self.getAutonomousActions(beliefs, agentGraph)
assert self.pos == oldPos
assert self.dir == oldDir
assert self.velocity == oldVel
if Car.DRIVE_FORWARD in actions:
percent = actions[Car.DRIVE_FORWARD]
percent = max(percent, 0.0)
percent = min(percent, 1.0)
self.accelerate(Junior.ACCELERATION * percent)
if Car.TURN_WHEEL in actions:
turnAngle = actions[Car.TURN_WHEEL]
self.setWheelAngle(turnAngle)
def drawBeliefSquare(row, col, color, value, model):
tileSize = Const.BELIEF_TILE_SIZE
x = col * tileSize + tileSize / 2.0
y = row * tileSize + tileSize / 2.0
if not model.inBounds(x, y): return None
color = Display._getBeliefSquareColor(color, value)
return Display.drawSquare(Vec2d(x, y), tileSize, color)
def decellerate(self, amount):
speed = self.velocity.get_length()
if speed == 0: return
frictionVec = self.velocity.get_reflection().normalized()
frictionVec *= amount
self.velocity += frictionVec
angle = self.velocity.get_angle_between(frictionVec)
if abs(angle) < 180:
self.velocity = Vec2d(0, 0)
def __init__(self, pos, dirName, velocity):
self.initialPos = Vec2d(pos.x, pos.y)
self.pos = pos
self.velocity = velocity
direction = self.dirFromName(dirName)
self.dir = direction
self.wheelAngle = 0
self.maxSpeed = Car.MAX_SPEED
self.friction = Car.FRICTION
self.maxWheelAngle = Car.MAX_WHEEL_ANGLE
def vecs_from_coords(coords):
vecs = []
x = None
for coord in coords:
if x == None:
x = coord
else:
vecs.append(Vec2d(x, coord))
x = None
return vecs
def rotate_by(obj, angle):
vecs = vecs_from_coords(_canvas.coords(obj))
anchorPos = Vec2d.getAverage(vecs)
newVecs = []
for vec in vecs:
vec -= anchorPos
vec.rotate(angle)
vec += anchorPos
newVecs.append(vec)
newCoords = coords_from_vecs(newVecs)
_canvas.coords(obj, *newCoords)
def heartbeat(self):
oldDir = Vec2d(self.junior.dir.x, self.junior.dir.y)
oldPos = Vec2d(self.junior.pos.x, self.junior.pos.y)
quitAction = self.junior.action()
carProb = self.model.getProbCar()
if carProb and Const.AUTO:
agentGraph = self.model.getJuniorGraph()
self.junior.autonomousAction(carProb, agentGraph)
if quitAction:
self.quit = True
return
self.junior.update()
self.collision = self.model.checkCollision(self.junior)
self.victory = self.model.checkVictory()
newPos = self.junior.getPos()
newDir = self.junior.getDir()
deltaPos = newPos - oldPos
deltaAngle = oldDir.get_angle_between(newDir)
Display.move(self.junior, deltaPos)
Display.rotate(self.junior, deltaAngle)
def rotate_by(obj, angle):
vecs = vecs_from_coords(_canvas.coords(obj))
anchorPos = Vec2d.getAverage(vecs)
newVecs = []
for vec in vecs:
vec -= anchorPos
vec.rotate(angle)
vec += anchorPos
newVecs.append(vec)
newCoords = coords_from_vecs(newVecs)
_canvas.coords(obj, *newCoords)
def __init__(self, layout, QLearner):
self._initBlocks(layout)
self._initIntersections(layout)
self.layout = layout
startX = layout.getStartX()
startY = layout.getStartY()
startDirName = layout.getJuniorDir()
self.junior = AutoDriver(QLearner)
self.junior.setup(Vec2d(startX, startY), startDirName, Vec2d(0, 0))
self.cars = [self.junior]
self.otherCars = []
self.finish = Block(layout.getFinish())
agentComm = AgentCommunication()
if self.layout.getWorldName() != 'highway':
agentGraph = layout.getAgentGraph()
for _ in range(Const.NUM_AGENTS):
startNode = self._getStartNode(agentGraph)
other = Agent(startNode, layout.getAgentGraph(), self,
agentComm)
self.cars.append(other)
self.otherCars.append(other)
else:
agentGraphCarB = layout.getAgentGraphCarB()
startNode = self._getStartNode(agentGraphCarB)
other = Agent(startNode, layout.getAgentGraphCarB(), self,
agentComm)
self.cars.append(other)
self.otherCars.append(other)
agentGraphTruck = layout.getAgentGraphTruck()
startNode = self._getStartNode(agentGraphTruck)
other = Agent(startNode, layout.getAgentGraphTruck(), self,
agentComm)
self.cars.append(other)
self.otherCars.append(other)
self.observations = []
agentComm.addAgents(self.otherCars)
self.modelLock = threading.Lock()
self.probCarSet = False
def __init__(self, startNode, agentGraph, model, agentComm):
self.agentGraph = agentGraph
self.model = model
self.agentComm = agentComm
startPos = startNode.getPos()
startDirName = startNode.getDir()
super(Agent, self).__init__(startPos, startDirName, Vec2d(0, 0))
self.maxSpeed = random.gauss(Car.MAX_SPEED, Agent.MAX_SPEED_STD)
self.goalNode = self.getGoalNode(startNode.getId())
self.hasInference = False
self.color = self.initColor()
self.inIntersection = True
self.claimedIntersection = None
def collides(self, otherPos, otherBounds):
diff = otherPos - self.pos
dist = diff.get_length()
if dist > Car.RADIUS * 2: return False
bounds = self.getBounds()
vec1 = bounds[0] - bounds[1]
vec2 = otherBounds[0] - otherBounds[1]
axis = [
vec1,
vec1.perpendicular(),
vec2,
vec2.perpendicular()
]
for vec in axis:
(minA, maxA) = Vec2d.projectPoints(bounds, vec)
(minB, maxB) = Vec2d.projectPoints(otherBounds, vec)
leftmostA = minA <= minB
overlap = False
if leftmostA and maxA >= minB: overlap = True
if not leftmostA and maxB >= minA: overlap = True
if not overlap: return False
return True
def rectangle(pos, length, width, color, dir=None, filled=1, behind=0):
coordVecs = [
Vec2d(-width / 2.0, -length / 2.0),
Vec2d(+width / 2.0, -length / 2.0),
Vec2d(+width / 2.0, +length / 2.0),
Vec2d(-width / 2.0, +length / 2.0)
]
if dir != None:
dir = dir.normalized()
protoDir = Vec2d(0, -1)
angle = -dir.get_angle_between(protoDir)
else:
angle = 0
coords = []
for coord in coordVecs:
coord.rotate(angle)
coord += pos
coords.append((coord.x, coord.y))
return polygon(coords, color, color, filled, 0, behind=behind)
def dirFromName(self, dirName):
if dirName == 'north': return Vec2d(0, -1)
if dirName == 'west': return Vec2d(-1, 0)
if dirName == 'south': return Vec2d(0, 1)
if dirName == 'east': return Vec2d(1, 0)
raise Exception(str(dirName) + ' is not a recognized dir.')
def getCenter(self):
return Vec2d(self.centerX, self.centerY)
def getPos(self):
return Vec2d(self.x, self.y)
def turnCarTowardsWheels(self):
if self.velocity.get_length() > 0.0:
self.velocity.rotate(self.wheelAngle)
self.dir = Vec2d(self.velocity.x, self.velocity.y)
