class Board: # rCount = roomba count, sCount = spike roomba count def __init__(self, width, height, rCount, sCount): self.width = width # width of board in meters self.height = height # width of board in meters self.rC = [] # roomba list self.srC = [] # spike roomba list self.boardTime = TimeMultiplier(1) pixelspermeter = 30 # number of pixels per meter theta = 2*pi/rCount boardCenter = (width/2)*pixelspermeter+25 for x in range(rCount): # initialize roombas pos = Vector(boardCenter + 30*sin(theta*x), boardCenter + 30*cos(theta*x), 0) # put roombas in diagonal vel = Vector(sin(theta*x)/3, cos(theta*x)/3, 0) # random velocities (magnitude 1 m/s) rCircle = Circle(Point(pos.x, pos.y), 9.0/2) # creating circle object for roomba rLine = Line(Point(pos.x,pos.y), Point(pos.x+vel.x*50, pos.y+vel.y*50)) # velcoity vector line self.rC.append(Roomba(pos, vel, rCircle, rLine, self.boardTime)) # add new Roomba object to list for i in range(sCount): # initialize spike roombas at theta = 0, pi/2, pi, and 3pi/2 pos = Vector(30 * width / 2 + 25 + cos(i*pi/2) * 5 * 30,30 * height / 2 + 25 + sin(i*pi/2) * 5 * 30, 0) vel = Vector(-sin(i*pi/2)/3, cos(i*pi/2)/3, 0) rCircle = Circle(Point(pos.x, pos.y), 9.0/2) # creating spike roomba circle object self.srC.append(Spike(pos, vel, rCircle, i, self.boardTime)) # add new Spike object to list self.rCsrC = self.rC + self.srC #UAV initialization pos = Vector(boardCenter,boardCenter,0) vel = Vector(0,0,0) rCircle = Circle(Point(pos.x,pos.y), 6.0) maxSpeed = 2 # in m/s self.uav = UAV(pos, vel, rCircle, maxSpeed, self.boardTime, self.rC, self.srC) # Draws the board and roombas def draw(self): pixelspermeter = 30 # number of pixels per meter self.pxwidth = self.width*pixelspermeter # pixel width of active board - the 30 is completely arbitrary self.pxheight = self.height*pixelspermeter # pixel height of active board buff = 50 # buffer around board self.win = GraphWin("My Board", self.pxwidth+buff, self.pxheight+buff, False) # creating a board w/ a 25 pixel buffer on each side # Four corner points upperleft = Point(buff/2, buff/2) upperright = Point(buff/2+self.pxwidth, buff/2) lowerright = Point(buff/2+self.pxwidth, buff/2 + self.pxheight) lowerleft = Point(buff/2, buff/2 + self.pxheight) # entire boundary boundary = Rectangle(upperleft, lowerright) # boundary of the grid boundary.setOutline(color_rgb(255, 255, 255)) # white boundary # grid blockwidth = pixelspermeter blockheight = pixelspermeter vertarray = [Line(Point(buff/2+i*blockwidth, buff/2), Point(buff/2+i*blockwidth, buff/2+self.pxheight)) for i in range(self.width)] # vertical lines horizarray = [Line(Point(buff/2, buff/2+j*blockheight), Point(buff/2+self.pxwidth, buff/2+j*blockheight)) for j in range(self.height)] # horizontal lines linearray = vertarray + horizarray # all lines for line in linearray: # color each line black line.setOutline(color_rgb(255, 255, 255)) # upper green line topline = Line(upperleft, upperright) topline.setOutline(color_rgb(0, 255, 0)) # lower red line botline = Line(lowerleft, lowerright) botline.setOutline(color_rgb(255, 0, 0)) # draw the boundary and all lines boundary.draw(self.win) for line in linearray: # draw grid lines line.draw(self.win) topline.draw(self.win) botline.draw(self.win) for r in self.rC: # draw roombas r.circle.setFill(color_rgb(0, 0, 0)) r.circle.draw(self.win) r.velVect.draw(self.win) print r.velVect for r in self.srC: # draw spike roombas r.circle.setFill(color_rgb(0,0,150)) r.circle.draw(self.win) # draw UAV self.uav.circle.setFill(color_rgb(150,0,0)) self.uav.circle.draw(self.win) self.win.setBackground(color_rgb(150, 150, 150)) # grey background #self.win.getMouse() # pause for click in self.window # Collision check: if distance between roomba centers is less than a threshold, they've collided # 1 for collision, 0 for no collision def collision(self, r, otherPos): threshold = 64 x = r.pos.x + r.vel.x*12 y = r.pos.y + r.vel.y*12 dx = x - otherPos.x dy = y - otherPos.y if threshold > dx*dx + dy*dy: return True else: return False # the primary method for running the simulation def run(self): self.draw() lastTime = self.boardTime.getTime() while len(self.srC) > 0: # while there are still roombas left, keep running timeInterval = self.boardTime.getTime()-lastTime lastTime = self.boardTime.getTime() for r in self.rC: if r.d == 1: # if the roomba is dead, remove it r.circle.undraw() self.rC.remove(r) self.rCsrC.remove(r) if self.uav.d == 1: # remove dead UAV self.uav.circle.undraw() cDetect = [] # list of roombas pairs for x in range(len(self.rCsrC)): # generates all possible unique pairs of roombas d = range(x, len(self.rCsrC)) e = [d.pop(0)] * len(d) cDetect = cDetect + zip(e, d) for x in cDetect: if self.collision(self.rCsrC[x[0]], self.rCsrC[x[1]].pos): # if there is a collision, reverse directions of both roombas if self.rCsrC[x[0]] in self.rC and self.rCsrC[x[0]].turning == False: self.rC[x[0]].flip() if self.collision(self.rCsrC[x[1]], self.rCsrC[x[0]].pos): if self.rCsrC[x[1]] in self.rC and self.rCsrC[x[1]].turning == False: self.rC[x[1]].flip() for r in self.rCsrC : # draw updated roombas # moves roombas with move function - uses less processing time #r.circle.move(timeInterval*r.vel.x*30, timeInterval*r.vel.y*30) # moves roombas by drawing and undrawing - laggy r.circle.undraw() r.circle.draw(self.win) if r in self.rC: r.velVect.undraw() r.velVect.draw(self.win) r.step() #Point(r.pos.x,r.pos.y).draw(self.win) # traces roomba path - laggy # draw updated UAV #self.uav.circle.move(timeInterval*self.uav.vel.x*30, timeInterval*self.uav.vel.y*30) self.uav.circle.undraw() self.uav.circle.draw(self.win) #Point(self.uav.pos.x,self.uav.pos.y).draw(self.win) #self.uav.update(self.rC,self.srC) self.uav.step() self.win.update() self.stop() # quit the simulation def stop(self): self.win.getMouse()
class Board: # rCount = roomba count, sCount = spike roomba count def __init__(self, width, height, rData, sData): self.width = width # width of board in meters self.height = height # width of board in meters self.rC = [] # roomba list self.srC = [] # spike roomba list pixelspermeter = 30 # number of pixels per meter theta = 2*pi/rCount boardCenter = (width/2)*pixelspermeter+25 for x in range(rCount): # initialize roombas pos = Vector(boardCenter + 30*sin(theta*x), boardCenter + 30*cos(theta*x), 0) # put roombas in diagonal vel = Vector(sin(theta*x)/3, cos(theta*x)/3, 0) # random velocities (magnitude 1 m/s) rCircle = Circle(Point(pos.x, pos.y), 9.0/2) # creating circle object for roomba rLine = Line(Point(pos.x,pos.y), Point(pos.x+vel.x*50, pos.y+vel.y*50)) # velcoity vector line self.rC.append(uavRoomba(pos, vel, rCircle, rLine, self.boardTime)) # add new Roomba object to list for i in range(sCount): # initialize spike roombas at theta = 0, pi/2, pi, and 3pi/2 pos = Vector(30 * width / 2 + 25 + cos(i*pi/2) * 5 * 30,30 * height / 2 + 25 + sin(i*pi/2) * 5 * 30, 0) vel = Vector(-sin(i*pi/2)/3, cos(i*pi/2)/3, 0) rCircle = Circle(Point(pos.x, pos.y), 9.0/2) # creating spike roomba circle object self.srC.append(uavSpike(pos, vel, rCircle, i, self.boardTime)) # add new Spike object to list self.rCsrC = self.rC + self.srC #UAV initialization pos = Vector(boardCenter,boardCenter,0) vel = Vector(0,0,0) rCircle = Circle(Point(pos.x,pos.y), 6.0) maxSpeed = 2 # in m/s self.uav = UAV(pos, vel, rCircle, maxSpeed, self.boardTime, self.rC, self.srC) # Collision check: if distance between roomba centers is less than a threshold, they've collided # 1 for collision, 0 for no collision def collision(self, r, otherPos): threshold = 64 x = r.pos.x + r.vel.x*12 y = r.pos.y + r.vel.y*12 dx = x - otherPos.x dy = y - otherPos.y if threshold > dx*dx + dy*dy: return True else: return False # the primary method for running the simulation def run(self): self.draw() lastTime = self.boardTime.getTime() while len(self.srC) > 0: # while there are still roombas left, keep running timeInterval = self.boardTime.getTime()-lastTime lastTime = self.boardTime.getTime() for r in self.rC: if r.d == 1: # if the roomba is dead, remove it r.circle.undraw() self.rC.remove(r) self.rCsrC.remove(r) if self.uav.d == 1: # remove dead UAV self.uav.circle.undraw() cDetect = [] # list of roombas pairs for x in range(len(self.rCsrC)): # generates all possible unique pairs of roombas d = range(x, len(self.rCsrC)) e = [d.pop(0)] * len(d) cDetect = cDetect + zip(e, d) for x in cDetect: if self.collision(self.rCsrC[x[0]], self.rCsrC[x[1]].pos): # if there is a collision, reverse directions of both roombas if self.rCsrC[x[0]] in self.rC and self.rCsrC[x[0]].turning == False: self.rC[x[0]].flip() if self.collision(self.rCsrC[x[1]], self.rCsrC[x[0]].pos): if self.rCsrC[x[1]] in self.rC and self.rCsrC[x[1]].turning == False: self.rC[x[1]].flip() for r in self.rCsrC : # draw updated roombas # moves roombas with move function - uses less processing time #r.circle.move(timeInterval*r.vel.x*30, timeInterval*r.vel.y*30) # moves roombas by drawing and undrawing - laggy r.circle.undraw() r.circle.draw(self.win) if r in self.rC: r.velVect.undraw() r.velVect.draw(self.win) r.step() #Point(r.pos.x,r.pos.y).draw(self.win) # traces roomba path - laggy # draw updated UAV #self.uav.circle.move(timeInterval*self.uav.vel.x*30, timeInterval*self.uav.vel.y*30) self.uav.circle.undraw() self.uav.circle.draw(self.win) #Point(self.uav.pos.x,self.uav.pos.y).draw(self.win) #self.uav.update(self.rC,self.srC) self.uav.step() self.win.update() self.stop() # quit the simulation def stop(self): self.win.getMouse()
class Board: # rCount = roomba count, sCount = spike roomba count def __init__(self, width, height, rCount, sCount): self.width = width # width of board in meters self.height = height # width of board in meters self.rC = [] # roomba list self.srC = [] # spike roomba list self.boardTime = TimeMultiplier(1) pixelspermeter = 30 # number of pixels per meter theta = 2 * pi / rCount boardCenter = (width / 2) * pixelspermeter + 25 for x in range(rCount): # initialize roombas pos = Vector(boardCenter + 30 * sin(theta * x), boardCenter + 30 * cos(theta * x), 0) # put roombas in diagonal vel = Vector(sin(theta * x) / 3, cos(theta * x) / 3, 0) # random velocities (magnitude 1 m/s) rCircle = Circle(Point(pos.x, pos.y), 9.0 / 2) # creating circle object for roomba rLine = Line(Point(pos.x, pos.y), Point(pos.x + vel.x * 50, pos.y + vel.y * 50)) # velcoity vector line self.rC.append( Roomba(pos, vel, rCircle, rLine, self.boardTime)) # add new Roomba object to list for i in range( sCount ): # initialize spike roombas at theta = 0, pi/2, pi, and 3pi/2 pos = Vector(30 * width / 2 + 25 + cos(i * pi / 2) * 5 * 30, 30 * height / 2 + 25 + sin(i * pi / 2) * 5 * 30, 0) vel = Vector(-sin(i * pi / 2) / 3, cos(i * pi / 2) / 3, 0) rCircle = Circle(Point(pos.x, pos.y), 9.0 / 2) # creating spike roomba circle object self.srC.append( Spike(pos, vel, rCircle, i, self.boardTime)) # add new Spike object to list self.rCsrC = self.rC + self.srC #UAV initialization pos = Vector(boardCenter, boardCenter, 0) vel = Vector(0, 0, 0) rCircle = Circle(Point(pos.x, pos.y), 6.0) maxSpeed = 2 # in m/s self.uav = UAV(pos, vel, rCircle, maxSpeed, self.boardTime, self.rC, self.srC) # Draws the board and roombas def draw(self): pixelspermeter = 30 # number of pixels per meter self.pxwidth = self.width * pixelspermeter # pixel width of active board - the 30 is completely arbitrary self.pxheight = self.height * pixelspermeter # pixel height of active board buff = 50 # buffer around board self.win = GraphWin( "My Board", self.pxwidth + buff, self.pxheight + buff, False) # creating a board w/ a 25 pixel buffer on each side # Four corner points upperleft = Point(buff / 2, buff / 2) upperright = Point(buff / 2 + self.pxwidth, buff / 2) lowerright = Point(buff / 2 + self.pxwidth, buff / 2 + self.pxheight) lowerleft = Point(buff / 2, buff / 2 + self.pxheight) # entire boundary boundary = Rectangle(upperleft, lowerright) # boundary of the grid boundary.setOutline(color_rgb(255, 255, 255)) # white boundary # grid blockwidth = pixelspermeter blockheight = pixelspermeter vertarray = [ Line(Point(buff / 2 + i * blockwidth, buff / 2), Point(buff / 2 + i * blockwidth, buff / 2 + self.pxheight)) for i in range(self.width) ] # vertical lines horizarray = [ Line(Point(buff / 2, buff / 2 + j * blockheight), Point(buff / 2 + self.pxwidth, buff / 2 + j * blockheight)) for j in range(self.height) ] # horizontal lines linearray = vertarray + horizarray # all lines for line in linearray: # color each line black line.setOutline(color_rgb(255, 255, 255)) # upper green line topline = Line(upperleft, upperright) topline.setOutline(color_rgb(0, 255, 0)) # lower red line botline = Line(lowerleft, lowerright) botline.setOutline(color_rgb(255, 0, 0)) # draw the boundary and all lines boundary.draw(self.win) for line in linearray: # draw grid lines line.draw(self.win) topline.draw(self.win) botline.draw(self.win) for r in self.rC: # draw roombas r.circle.setFill(color_rgb(0, 0, 0)) r.circle.draw(self.win) r.velVect.draw(self.win) print r.velVect for r in self.srC: # draw spike roombas r.circle.setFill(color_rgb(0, 0, 150)) r.circle.draw(self.win) # draw UAV self.uav.circle.setFill(color_rgb(150, 23, 189)) self.uav.circle.draw(self.win) self.win.setBackground(color_rgb(150, 150, 150)) # grey background #self.win.getMouse() # pause for click in self.window # Collision check: if distance between roomba centers is less than a threshold, they've collided # 1 for collision, 0 for no collision def collision(self, r, otherPos): threshold = 64 x = r.pos.x + r.vel.x * 12 y = r.pos.y + r.vel.y * 12 dx = x - otherPos.x dy = y - otherPos.y if threshold > dx * dx + dy * dy: return True else: return False # the primary method for running the simulation def run(self): self.draw() lastTime = self.boardTime.getTime() while len(self.srC ) > 0: # while there are still roombas left, keep running timeInterval = self.boardTime.getTime() - lastTime lastTime = self.boardTime.getTime() for r in self.rC: if r.d == 1: # if the roomba is dead, remove it r.circle.undraw() self.rC.remove(r) self.rCsrC.remove(r) if self.uav.d == 1: # remove dead UAV self.uav.circle.undraw() cDetect = [] # list of roombas pairs for x in range(len(self.rCsrC) ): # generates all possible unique pairs of roombas d = range(x, len(self.rCsrC)) e = [d.pop(0)] * len(d) cDetect = cDetect + zip(e, d) for x in cDetect: if self.collision( self.rCsrC[x[0]], self.rCsrC[x[1]].pos ): # if there is a collision, reverse directions of both roombas if self.rCsrC[x[0]] in self.rC and self.rCsrC[ x[0]].turning == False: self.rC[x[0]].flip() if self.collision(self.rCsrC[x[1]], self.rCsrC[x[0]].pos): if self.rCsrC[x[1]] in self.rC and self.rCsrC[ x[1]].turning == False: self.rC[x[1]].flip() for r in self.rCsrC: # draw updated roombas # moves roombas with move function - uses less processing time #r.circle.move(timeInterval*r.vel.x*30, timeInterval*r.vel.y*30) # moves roombas by drawing and undrawing - laggy r.circle.undraw() r.circle.draw(self.win) if r in self.rC: r.velVect.undraw() r.velVect.draw(self.win) r.step() #Point(r.pos.x,r.pos.y).draw(self.win) # traces roomba path - laggy # draw updated UAV #self.uav.circle.move(timeInterval*self.uav.vel.x*30, timeInterval*self.uav.vel.y*30) self.uav.circle.undraw() self.uav.circle.draw(self.win) #Point(self.uav.pos.x,self.uav.pos.y).draw(self.win) #self.uav.update(self.rC,self.srC) self.uav.step() self.win.update() self.stop() # quit the simulation def stop(self): self.win.getMouse()
X = np.zeros((int((T - t0) / h) + 1,2)) Psi = np.zeros((int((T - t0) / h) + 1,2)) t = np.zeros(int((T - t0) / h) + 1) u = np.zeros(int((T - t0) / h) + 1) gamma = 0 i = 0 gamma = -np.pi / 2 for s in np.arange(t0, T + h, h): #if s < T/2: # gamma = gamma - 2.0 * np.pi * h / T #else: # gamma = gamma + 2.0 * np.pi * h / T gamma = gamma + 2.0 * np.pi * h / T t[i] = s x = uav.step(gamma) #u[i] = uav.u(x) X[i] = x Psi[i] = [np.cos(gamma), np.sin(gamma)] #X[i] = (XT-X0) * h/T * i #Psi[i] = [np.cos(gamma), np.sin(gamma)] i = i + 1 # XT = X[-1] def dFdX(t, X,kappa): return target.dnu(t,X) * np.log(1 + channel.l(X) * uav.u(t,X)) + target.nu(t,X) * (channel.dl(X) * uav.u(t,X) + channel.l(X) * target.dnu(t,X)) / (1 + channel.l(X) * uav.u(t,X)) - kappa * uav.du(t,X) def dXdPsi(Psi,V):
class Board: # rCount = roomba count, sCount = spike roomba count def __init__(self, width, height, rData, sData): self.width = width # width of board in meters self.height = height # width of board in meters self.rC = [] # roomba list self.srC = [] # spike roomba list pixelspermeter = 30 # number of pixels per meter theta = 2 * pi / rCount boardCenter = (width / 2) * pixelspermeter + 25 for x in range(rCount): # initialize roombas pos = Vector(boardCenter + 30 * sin(theta * x), boardCenter + 30 * cos(theta * x), 0) # put roombas in diagonal vel = Vector(sin(theta * x) / 3, cos(theta * x) / 3, 0) # random velocities (magnitude 1 m/s) rCircle = Circle(Point(pos.x, pos.y), 9.0 / 2) # creating circle object for roomba rLine = Line(Point(pos.x, pos.y), Point(pos.x + vel.x * 50, pos.y + vel.y * 50)) # velcoity vector line self.rC.append( uavRoomba(pos, vel, rCircle, rLine, self.boardTime)) # add new Roomba object to list for i in range( sCount ): # initialize spike roombas at theta = 0, pi/2, pi, and 3pi/2 pos = Vector(30 * width / 2 + 25 + cos(i * pi / 2) * 5 * 30, 30 * height / 2 + 25 + sin(i * pi / 2) * 5 * 30, 0) vel = Vector(-sin(i * pi / 2) / 3, cos(i * pi / 2) / 3, 0) rCircle = Circle(Point(pos.x, pos.y), 9.0 / 2) # creating spike roomba circle object self.srC.append( uavSpike(pos, vel, rCircle, i, self.boardTime)) # add new Spike object to list self.rCsrC = self.rC + self.srC #UAV initialization pos = Vector(boardCenter, boardCenter, 0) vel = Vector(0, 0, 0) rCircle = Circle(Point(pos.x, pos.y), 6.0) maxSpeed = 2 # in m/s self.uav = UAV(pos, vel, rCircle, maxSpeed, self.boardTime, self.rC, self.srC) # Collision check: if distance between roomba centers is less than a threshold, they've collided # 1 for collision, 0 for no collision def collision(self, r, otherPos): threshold = 64 x = r.pos.x + r.vel.x * 12 y = r.pos.y + r.vel.y * 12 dx = x - otherPos.x dy = y - otherPos.y if threshold > dx * dx + dy * dy: return True else: return False # the primary method for running the simulation def run(self): self.draw() lastTime = self.boardTime.getTime() while len(self.srC ) > 0: # while there are still roombas left, keep running timeInterval = self.boardTime.getTime() - lastTime lastTime = self.boardTime.getTime() for r in self.rC: if r.d == 1: # if the roomba is dead, remove it r.circle.undraw() self.rC.remove(r) self.rCsrC.remove(r) if self.uav.d == 1: # remove dead UAV self.uav.circle.undraw() cDetect = [] # list of roombas pairs for x in range(len(self.rCsrC) ): # generates all possible unique pairs of roombas d = range(x, len(self.rCsrC)) e = [d.pop(0)] * len(d) cDetect = cDetect + zip(e, d) for x in cDetect: if self.collision( self.rCsrC[x[0]], self.rCsrC[x[1]].pos ): # if there is a collision, reverse directions of both roombas if self.rCsrC[x[0]] in self.rC and self.rCsrC[ x[0]].turning == False: self.rC[x[0]].flip() if self.collision(self.rCsrC[x[1]], self.rCsrC[x[0]].pos): if self.rCsrC[x[1]] in self.rC and self.rCsrC[ x[1]].turning == False: self.rC[x[1]].flip() for r in self.rCsrC: # draw updated roombas # moves roombas with move function - uses less processing time #r.circle.move(timeInterval*r.vel.x*30, timeInterval*r.vel.y*30) # moves roombas by drawing and undrawing - laggy r.circle.undraw() r.circle.draw(self.win) if r in self.rC: r.velVect.undraw() r.velVect.draw(self.win) r.step() #Point(r.pos.x,r.pos.y).draw(self.win) # traces roomba path - laggy # draw updated UAV #self.uav.circle.move(timeInterval*self.uav.vel.x*30, timeInterval*self.uav.vel.y*30) self.uav.circle.undraw() self.uav.circle.draw(self.win) #Point(self.uav.pos.x,self.uav.pos.y).draw(self.win) #self.uav.update(self.rC,self.srC) self.uav.step() self.win.update() self.stop() # quit the simulation def stop(self): self.win.getMouse()