diff --git a/Action.py b/Action.py index 492a478..d6db6bc 100644 --- a/Action.py +++ b/Action.py @@ -17,7 +17,7 @@ def __init__(self, agent, duration): Constructor ''' self.agentToStun = agent - self.agentUID = agent.getUID() + self.agentUID = agent.uid self.duration = duration @@ -25,7 +25,7 @@ class Kick(object): def __init__(self, ball, direction, intensity): self.ball = ball - self.ballUID = ball.getUID() + self.ballUID = ball.uid self.direction = direction self.intensity = intensity \ No newline at end of file diff --git a/Agent.py b/Agent.py index e8c4ce8..d8547ed 100644 --- a/Agent.py +++ b/Agent.py @@ -26,11 +26,12 @@ class Agent(object): def __init__(self, team, position, rotation, brain, colRadius, drawRadius): self.position = position.astype(float) #numpy array [x, y ,z] self.rotation = rotation.astype(float) #numpy array [yaw, pitch, roll] (in degrees) + self.rotMatrix = rotMatrixFromYPR(self.rotation) self.colRadius = colRadius #float size of collision sphere self.drawRadius = drawRadius #float size of sphere to be drawn self.team = team #provide team 'A' or team 'B' - self.forward = dot(array([1, 0, 0]), rotMatrixFromYPR(rotation)) #unit vector in forward direction of agent - self.right = dot(array([0, 1, 0]), rotMatrixFromYPR(rotation)) #unit vector in right direction of agent + self.forward = dot(array([1, 0, 0]), self.rotMatrix) #unit vector in forward direction of agent + self.right = dot(array([0, 1, 0]), self.rotMatrix) #unit vector in right direction of agent self.up = cross(self.forward, self.right) #unit vector pointing upwards self.maxMove = double(0.6666) #max distance the agent can move in each frame self.maxRot = array([5, 5, 5]) #max YPR in degrees the agent can rotate in each frame @@ -87,12 +88,7 @@ def draw(self, subplot): Returns egocentric position of other object with respect to self ''' def getEgoCentricOf(self, otherObject): - otherPosition = otherObject.position; - rotMat = rotMatrixFromYPR(self.rotation) - rotMatInverse = inv(rotMat) - posVector = otherPosition - self.position - egoCentric = dot(posVector, rotMatInverse) - return egoCentric + return dot(otherObject.position - self.position, inv(self.rotMatrix)) ''' Moves the agent, given information about the world, places restrictions on motion, called by the simulator. @@ -112,13 +108,13 @@ def moveAgent(self, world): #handle stun action if action.__class__.__name__ == 'Stun': for agent in world.agents: - if agent.getUID() == action.agentUID: + if agent.uid == action.agentUID: if distBetween(self.position, agent.position) < self.stunRange: agent.stun(action.duration) #handle kick action if action.__class__.__name__ == 'Kick': for ball in world.balls: - if ball.getUID() == action.ballUID: + if ball.uid == action.ballUID: if distBetween(self.position, ball.position) < 20: globalDirection = dot(action.direction, rotMatrixFromYPR(self.rotation)) ball.kick(globalDirection, action.intensity) @@ -141,15 +137,18 @@ def buildEgoCentricRepresentationOfWorld(self, world): obstacles =[] for agent in world.agents: if agent != self: - agentToAppend = Agent(agent.team, self.getEgoCentricOf(agent), agent.rotation - self.rotation, agent.brain, agent.colRadius, agent.drawRadius) - agentToAppend.setUID(agent.getUID()) + agentToAppend = LiteAgent(agent.team, + self.getEgoCentricOf(agent), + agent.rotation - self.rotation, + agent.colRadius, agent.drawRadius, + agent.uid) if agent.team == self.team: myTeam.append(agentToAppend) else: enemyTeam.append(agentToAppend) for ball in world.balls: ballToAppend = Ball(self.getEgoCentricOf(ball)) - ballToAppend.setUID(ball.getUID()) + ballToAppend.uid = ball.uid balls.append(ballToAppend) for obstacle in world.obstacles: obstacleToAppend = Obstacle(self.getEgoCentricOf(obstacle), obstacle.radius) @@ -164,6 +163,7 @@ def rotateAgent(self, rotation): if not self.isStunned: rotation = clampRotation(rotation, self.maxRot) self.rotation += rotation + self.rotMatrix = rotMatrixFromYPR(self.rotation) self.forward = normalize(dot(array([1, 0, 0]), rotMatrixFromYPR(self.rotation))) self.right = normalize(dot(array([0, 1, 0]), rotMatrixFromYPR(self.rotation))) self.up = normalize(cross(self.forward, self.right)) @@ -182,11 +182,14 @@ def stun(self, duration): self.isStunned = True self.lastStunned = SimTime.time self.stunDuration = duration - - def setUID(self, uid): - self.uid = uid - - def getUID(self): - return self.uid - \ No newline at end of file + +class LiteAgent(object): + __slots__ = ['team', 'position', 'rotation', 'colRadius', 'drawRadius', 'uid','getUID'] + def __init__(self, team, position, rotation, colRadius, drawRadius, uid): + self.team = team + self.position = position + self.rotation = rotation + self.colRadius = colRadius + self.drawRadius = drawRadius + self.uid = uid diff --git a/Ball.py b/Ball.py index feceec8..e30beea 100644 --- a/Ball.py +++ b/Ball.py @@ -98,12 +98,5 @@ def kick(self, direction, intensity): directionNorm = normalize(direction) if self.isDynamic: self.velocity = directionNorm * intensity - - def getUID(self): - return self.uid - - def setUID(self, uid): - self.uid = uid - - + \ No newline at end of file diff --git a/LinearAlegebraUtils.py b/LinearAlegebraUtils.py index 54b2123..886d089 100644 --- a/LinearAlegebraUtils.py +++ b/LinearAlegebraUtils.py @@ -9,15 +9,19 @@ from numpy import * import numpy as np +from math import sin, cos, radians def rotMatrixFromYPR(rotation): - a = math.radians(rotation[0]) - b = math.radians(rotation[1]) - c = math.radians(rotation[2]) + a = radians(rotation[0]) + b = radians(rotation[1]) + c = radians(rotation[2]) + + co = [cos(a),cos(b),cos(c)] + si = [sin(a),sin(b),sin(c)] + rotMat = array([ - [math.cos(a)*math.cos(b), math.cos(a)*math.sin(b)*math.sin(c) - math.sin(a)*math.cos(c), math.cos(a)*math.sin(b)*math.cos(c) + math.sin(a)*math.sin(c)], - [math.sin(a)*math.cos(b), math.sin(a)*math.sin(b)*math.sin(c) + math.cos(a)*math.cos(c), math.sin(a)*math.sin(b)*math.cos(c) - math.cos(a)*math.sin(c)], - [-math.sin(b), math.cos(b)*math.sin(c), math.cos(b)*math.cos(c)]]) - rotMat = transpose(rotMat) + [co[0]*co[1], si[0]*co[1], -si[1]], + [co[0]*si[1]*si[2] - si[0]*co[2], si[0]*si[1]*si[2] + co[0]*co[2], co[1]*si[2]], + [co[0]*si[1]*co[2] + si[0]*si[2], si[0]*si[1]*co[2] - co[0]*si[2], co[1]*co[2]]]) return rotMat def getYPRFromVector(vector): diff --git a/Simulator.py b/Simulator.py index e18fe90..239e14d 100644 --- a/Simulator.py +++ b/Simulator.py @@ -18,6 +18,8 @@ from RunAtBallBrain import RunAtBallBrain from Team import Team from SimTime import SimTime +from PIL import Image +import time @@ -36,6 +38,7 @@ def __init__(self, world, simTime, fps, imageDirName): self.imageDirName = imageDirName self.currWP = 0 self.ballWPs = [array([50.0, -100.0, 0.0]), array([0.0, 100.0, -70.0]), array([50.0, 20.0, 100.0]),array([-30.0, 50.0, -100.0]), array([80.0, -50.0, 50.0]), array([80.0, -50.0, -50.0]), array([-65.0, 20.0, 50.0]), array([-50.0, 20.0, -60.0])] + self.ps = [] def setup(self): #setup directory to save the images @@ -136,6 +139,8 @@ def run(self): physicsIndex+=1 currTime+=double(timeStep) + for p in self.ps: + p.join() print "Physics ran for "+str(physicsIndex)+" steps" print "Drawing ran for "+str(drawIndex)+" steps" @@ -149,25 +154,35 @@ def drawFrame(self, loopIndex): fname = self.imageDirName + '/' + str(int(100000000+loopIndex)) + '.png' # name the file self.loop(ax) plt.gca().set_ylim(ax.get_ylim()[::-1]) - savefig(fname, format='png', bbox_inches='tight') - print 'Written Frame No.'+ str(loopIndex)+' to '+ fname + # draw the renderer + fig.canvas.draw() + # Get the RGBA buffer from the figure + w,h = fig.canvas.get_width_height() + buf = np.fromstring ( fig.canvas.tostring_argb(), dtype=np.uint8 ) plt.close() + buf.shape = ( w, h,4 ) + # canvas.tostring_argb give pixmap in ARGB mode. Roll the ALPHA channel to have it in RGBA mode + buf = np.roll( buf, 3, axis = 2 )#(161,96)(1150,862) + img = Image.fromstring( "RGBA", ( w ,h ), buf.tostring() ) + img.crop((220, 116, 1150, 862)).save(fname) + print 'Written Frame No.'+ str(loopIndex)+' to '+ fname - -#Simulation runs here -#set the size of the world -world = World(100, 100) -#specify which world to simulate, total simulation time, and frammerate for video -sim = Simulator(world, 120, 30, "images") -#run the simulation -sim.run() - -''' -To create a video using the image sequence, execute the following command in command line. ->ffmpeg -framerate 30 -i "1%08d.png" -r 30 outPut.mp4 - ^ ^ - Framerate mtached with simulator -Make sure to set your current working directory to /images and have ffmpeg in your path. -''' - - \ No newline at end of file +if __name__=='__main__': + start_time = time.time() + #Simulation runs here + #set the size of the world + world = World(100, 100) + #specify which world to simulate, total simulation time, and frammerate for video + sim = Simulator(world, 1, 30, "images") + #run the simulation + sim.run() + print 'The program ran for {} seconds'.format(time.time()-start_time) + ''' + To create a video using the image sequence, execute the following command in command line. + >ffmpeg -framerate 30 -i "1%08d.png" -r 30 outPut.mp4 + ^ ^ + Framerate mtached with simulator + Make sure to set your current working directory to /images and have ffmpeg in your path. + ''' + + \ No newline at end of file