def __init__ (self, x, y):
self.x = x
self.y = y
# walk timer
self.timer = None
# smooth walk utils
self.__x = 0
self.__y = 0
self.offset_x = 0
self.offset_y = 0
self.__offset_x = 0
self.__offset_y = 0
# default orientation
self.ori = 'up'
# sprite
self.tileset = pygame.image.load('assets/opengameart/liberated pixel cup/princess.png')
self.images = []
for i in range (0, 4):
for j in range (0, 9):
self.images.append(self.tileset.subsurface(j * 64, i * 64, 64, 64))
# animations
self.up = animation(self.images[0:9])
self.left = animation(self.images[10:18])
self.down = animation(self.images[18:27])
self.right = animation(self.images[28:36])
def start_new_animation(self, a=None):
if(a==None):
self.animation = animation.animation(self.safe_control, self.animation_set[self.animation_iter])
self.animation_iter += 1
if(self.animation_iter > len(self.animation_set)-1):
self.animation_iter = 0
else:
self.animation = animation.animation(self.safe_control, a)
def harmonograph():
notes = noteFrequencies.noteFrequencies()
freqX=notes.get('c4','just')
freqY=notes.get('e4','just')
duration_in_samples = 900000
sample_rate = 44100.0
amplitude = 1000
damping = 0.2
hFunc = harmonicXYatT(freqX,freqY,amplitude, damping)
pygame.mixer.pre_init(int(sample_rate), -16, 1, 4096)
pygame.mixer.init()
makeNote = makeNoteFactory(duration_in_samples,sample_rate,amplitude,damping)
noteA = makeNote(freqX)
noteAsndarray = pygame.sndarray.make_sound(noteA)
noteAsound = pygame.mixer.Sound(noteAsndarray)
noteB = makeNote(freqY)
noteBsndarray = pygame.sndarray.make_sound(noteB)
noteBsound = pygame.mixer.Sound(noteBsndarray)
pygame.mixer.Sound.play(noteAsound)
pygame.mixer.Sound.play(noteBsound)
my_animation = animation.animation(hFunc)
my_animation.start()
def harmonograph(): hFunc = harmonicXYatT() duration_in_samples = 100000 sample_rate = 44100 amplitude = 10000 damping = .9 pygame.mixer.pre_init(sample_rate, -16, 1, 4096) pygame.mixer.init() makeNote = makeNoteFactory(duration_in_samples,sample_rate,amplitude,damping) a4 = makeNote(436.05) print a4 hNote = makeHarmonicNoteFromFunc(sample_rate,duration_in_samples) print hNote s2 = pygame.sndarray.make_sound(a4) sound2 = pygame.mixer.Sound(s2) s = pygame.sndarray.make_sound(hNote) sound = pygame.mixer.Sound(s) # pygame.mixer.Sound.play(sound) # pygame.mixer.Sound.play(sound2) my_animation = animation.animation(hFunc) my_animation.start()
def selectsprite(self, spritename, framedict, spritetilesize=32):
self.spritesheet = pygame.image.load(os.path.join(os.getcwd(), "Objects", spritename)).convert()
self.spritesheet.set_colorkey(self.spritesheet.get_at((0,0))) #may need to change this for sprites that do not use color key
self.spritesheet.convert_alpha()
#self.spritelist = self.imagecutter(self.spritesheet, spritetilesize)
self.playersprite.animate = animation.animation(self.spritesheet, framedict, "walk_up",
tilesize=(spritetilesize,spritetilesize))
def timer():
A = animation(**animation_args)
tc = 0.315
r = 3.0
lag = 0.1
for k in range(20):
theta = easing.OffsetEase(lag, stop=2 * np.pi, duration=len(A))()
L = line(
x0=0,
y0=0,
x1=r * np.cos(theta),
y1=r * np.sin(theta),
thickness=tc,
color='indigo',
)
A.add(L)
r *= 0.98
lag *= 1.17
return A
def rotating_circles():
A = animation(**animation_args)
x = easing.easeReturn('easeInOutQuad', -1, 1, len(A))
A.add(circle(x=x, y=1, r=1.25, color=[0, 250, 150]))
A.add(circle(x=-x, y=-1, r=1.25, color=[255, 5, 100]))
return A
def explode(self):
#reset box
for b in self.surrounding:
box.main_board.animations.add(
animation(
self, b,
self.main_board.speed)) # here need to create animation
self.owner.rem_box(self)
self.color = None # just to complete
self.holding = 0
def update(self) : # devide the operetion
if self.game_state == GAME_MODE['PLAY'] :
for eve in self.event :
if eve.type == KEYDOWN :
if eve.key == K_RIGHT or K_LEFT :
pygame.mouse.set_visible(False)
self.control = True
if eve.type == MOUSEMOTION :
pygame.mouse.set_visible(True)
self.control = False
self.play()
if self.flag[5] == True :
if not self.redist :
ex.redistr()
self.cards_reroad() #if player push the button 'call', cards reroad
self.redist = True
self.flag = [False] * 7
if self.flag[6] == True :
self.screen.fill(GREEN)
self.print_dist()
self.result()
self.print_deck()
pygame.display.update()
time.sleep(0.5)
self.voice()
self.flag = [False] * 7
elif self.game_state == GAME_MODE['DISTR'] :
self.cards_road() # cards images road here
an.animation(self.screen, GREEN, self.clock, self.cardback)
pygame.time.wait(1000)
self.redist = False
self.game_state = GAME_MODE['PLAY']
self.clock.tick(100)
elif self.game_state == GAME_MODE['START'] :
self.start()
elif self.game_state == GAME_MODE['RESULT'] :
self.print_deck()
self.print_dist()
self.game_skip()
def pacman():
args = animation_args.copy()
args["duration"] = 0.5
A = animation(**args)
pac_color = (253, 255, 0)
# Chomping easing function
dp = np.pi / 8
x0 = easing.easeOutQuad(0, dp, len(A) // 2)()
x1 = easing.easeInQuad(dp, 0, len(A) // 2)()
z = np.hstack([x0, x1])
pacman = ellipse(line_start=z, line_end=2 * np.pi - z, color=pac_color)
A.add(pacman)
return A
def checkerboard():
A = animation(**animation_args)
z = easing.easeReturn('easeInOutQuad', 0, 1, len(A))
r = 0.20
c = [150, 250, 0]
coord = [-2, 0, 2]
for dx, dy in itertools.product(coord, repeat=2):
A.add(circle(x=z + dx, y=z + dy, r=r, color=c))
A.add(circle(x=z + dx, y=-z + dy, r=r, color=c))
A.add(circle(x=-z + dx, y=-z + dy, r=r, color=c))
A.add(circle(x=-z + dx, y=z + dy, r=r, color=c))
A.add(circle(x=dx, y=z + dy, r=r, color=c))
A.add(circle(x=z + dx, y=dy, r=r, color=c))
A.add(circle(x=dx, y=-z + dy, r=r, color=c))
A.add(circle(x=-z + dx, y=dy, r=r, color=c))
return A
axs[0][0].plot(t, pos_x)
axs[0][0].set_ylabel('x(t)')
axs[1][0].plot(t, pos_y)
axs[1][0].set_ylabel('y(t)')
axs[2][0].plot(t, theta_block)
axs[2][0].set_ylabel('$\\theta_b$')
axs[3][0].plot(t, theta_tail)
axs[3][0].set_ylabel('$\\theta_t$')
axs[0][1].plot(t, vel_x)
axs[0][1].set_ylabel('$vel_x$')
axs[1][1].plot(t, vel_y)
axs[1][1].set_ylabel('$vel_y$')
axs[2][1].plot(t, omega_block)
axs[2][1].set_ylabel('$\omega_b(t)$')
axs[3][1].plot(t, omega_tail)
axs[3][1].set_ylabel('$\omega_t(t)$')
fig2 = plt.figure("Input Trajectory")
plt.plot(t, u)
plt.ylabel("torque")
plt.xlabel('Time (s)')
block_traj = []
tail_traj = []
for i in range(N):
block_traj.append((pos_x[i], pos_y[i], theta_block[i]))
tail_traj.append(theta_tail[i])
target_pt = (block_traj[-1][0], block_traj[-1][1])
animation(block_traj, tail_traj, target_pt)
[playerspritelist[36], playerspritelist[37], playerspritelist[38], playerspritelist[37]]]
playerdirection = 0 # for index into the player direction
playerstepphase = 0 # what animation frame to be on"""
glb.player1 = CharacterContainer("Dude 1", "test", True)
player1_frame_dict = {
"walk_down": [0, 1, 2, 1],
"walk_left": [12, 13, 14, 13],
"walk_right": [24, 25, 26, 25],
"walk_up": [36, 37, 38, 37],
"attack": [55, 56, 68, 67],
"battle_idle": [51, 52, 53, 52],
"enter": [51, 52, 53, 52]
}
glb.player1.animate = animation.animation(playerspritesheet,
player1_frame_dict,
"enter",
tilesize=(32, 32))
glb.player2 = CharacterContainer("Dude 2", "test", True)
player2_frame_dict = {
"attack": [55, 56, 68, 67],
"battle_idle": [51, 52, 53, 52],
"enter": [51, 52, 53, 52]
}
glb.player2.animate = animation.animation(playerspritesheet,
player2_frame_dict,
"enter",
tilesize=(32, 32))
glb.player3 = CharacterContainer("Dude 3", "test", True)
player3_frame_dict = {
def startGame1(DISPLAYSURF):
btn = Button('Final Vel,v')
btn2 = Button('Time, t')
btn3 = Button('In. Vel, u')
btn4 = Button('Accn., a')
btn5 = Button('Displacement, s')
clock = pygame.time.Clock()
background=pygame.image.load('Images/game1.jpg')
run = True
while run:
DISPLAYSURF.blit(background,(0,0))
mouse = pygame.mouse.get_pos()
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
elif event.type == pygame.MOUSEBUTTONDOWN:
if btn.obj.collidepoint(mouse):
countInputs=1;
u1 = inputbox.ask(DISPLAYSURF, "Initial velocity, u",countInputs)
u2 = float (u1)
countInputs=countInputs+5;
a1 = inputbox.ask(DISPLAYSURF, "Acceleration, a",countInputs)
a2 = float (a1)
countInputs=countInputs+5;
t1 = inputbox.ask(DISPLAYSURF, "Time, t",countInputs)
t2 = float (t1)
v1 = u2 + a2 * t2
v2 = str (v1)
font = pygame.font.Font(None, 36)
text = font.render("The Final Velocity hence calculated is "+v2+"metre/sec", 1, (10, 10, 10))
animation(DISPLAYSURF,'right',text)
elif btn2.obj.collidepoint(mouse):
countInputs=1;
u1 = inputbox.ask(DISPLAYSURF, "Initial Velocity, u",countInputs)
u2 = float (u1)
countInputs=countInputs+5;
a1 = inputbox.ask(DISPLAYSURF, "Acceleration, a",countInputs)
a2 = float (a1)
countInputs=countInputs+5;
v1 = inputbox.ask(DISPLAYSURF, "Final Velocity, t",countInputs)
v2 = float (v1)
t1 = (v2-u2)/a2
t2 = str (t1)
if(t1<0):
errorScreen(DISPLAYSURF,"Invalid datas entered")
font = pygame.font.Font(None, 36)
text = font.render("The Time hence calculated is "+t2+"seconds", 1, (10, 10, 10))
animation(DISPLAYSURF,'right',text)
elif btn3.obj.collidepoint(mouse):
countInputs=1;
v1 = inputbox.ask(DISPLAYSURF, "Final velocity, v",countInputs)
v2 = float (v1)
countInputs=countInputs+5;
a1 = inputbox.ask(DISPLAYSURF, "Acceleration, a",countInputs)
a2 = float (a1)
countInputs=countInputs+5;
t1 = inputbox.ask(DISPLAYSURF, "Time, t",countInputs)
t2 = float (t1)
u1 = v2 - (a2 * t2)
u2 = str (u1)
font = pygame.font.Font(None, 36)
text = font.render("The Initial Velocity hence calculated is "+u2+"metre/sec", 1, (10, 10, 10))
animation(DISPLAYSURF,'right',text)
elif btn4.obj.collidepoint(mouse):
countInputs=1;
u1 = inputbox.ask(DISPLAYSURF, "Initial velocity, u",countInputs)
u2 = float (u1)
countInputs=countInputs+5;
v1 = inputbox.ask(DISPLAYSURF, "Final velocity, v",countInputs)
v2 = float (v1)
countInputs=countInputs+5;
t1 = inputbox.ask(DISPLAYSURF, "Time, t",countInputs)
t2 = float (t1)
a1 = (v2 - u2)/t2
a2 = str (v1)
font = pygame.font.Font(None, 36)
text = font.render("The Acceleration hence calculated is "+a2+"metre/sec^2", 1, (10, 10, 10))
animation(DISPLAYSURF,'right',text)
btn.draw(DISPLAYSURF, mouse, (100,100,100,20), (125,103))
btn2.draw(DISPLAYSURF, mouse, (100,130,100,20), (125,133))
btn3.draw(DISPLAYSURF, mouse, (100,160,100,20), (125,163))
btn4.draw(DISPLAYSURF, mouse, (100,190,100,20), (125,193))
pygame.display.update()
clock.tick(60)
temps_sys = opts.temps_sys
if temps_sys <> 0 :
if temps_sys < temps_relatif * periode_affichage :
parser.error(u"Le temps d'évolution du système doit être supérieur \
à %.2f fois le coefficient de dilation du temps."
% periode_affichage)
size = opts.size
if size < 0 :
parser.error(u"La taille de la figure doit être strictement positive")
# Note : strictement, car si size = 0, on ajuste automatiquement.
# Donc si l'utilisateur a rentré quelque chose, il cherche à avoir une
# taille précise, et par conséquent size différent de 0.
systeme = Systeme(parse(sysfile))
if not size :
# On détermine la taille de la figure représentant le système
for corps in systeme.corps :
k = max(abs(corps.x),abs(corps.y))
if k > size :
size = k
size = size * 1.05 # Ajout de 5% pour ne pas avoir de points sur les bords
# On importe la fonction d'animation
from animation import animation
# Et c'est parti pour le show !
animation (systeme, periode_affichage, temps_relatif, wait,
calc_per_frame, method, temps_sys, size)
from animation import keyFrame, animation
import numpy as np
# basic example
anim = animation()
# making arrays for points
point2 = np.array(
[10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160])
point1 = np.array([
7, 14, 21, 28, 35, 42, 49, 56, 63, 70, 77, 84, 91, 98, 105, 112, 119, 126,
133, 140, 147, 154, 161, 168
])
# code to test spaces merging
spaces1 = np.zeros_like(point1)
spaces2 = np.zeros_like(point2)
spaces1[3] = 1
spaces1[5] = 1
spaces1[9] = 1
spaces2[1] = 1
#spaces2[4] = 1
#spaces2[7] = 1
#spaces2[9] = 1
#spaces2[13] = 1
#spaces2[15] = 1
u,
solver_parameters={
"newton_solver": {
"relative_tolerance": 9e-10,
"absolute_tolerance": 9e-10,
"maximum_iterations": 80
}
})
######################################################################
#making sure we start saving results after the pressure is imposed (if there is any pressure). Also making sure we save results every 10 steps
if sigma <= sigmax:
sigma += dsig
if sigma > sigmax:
if counter % 10 == 0:
animation(u, t, vtkfile11, vtkfile22, vtkfile33)
File("Z_Growth/displacement/u%d.xml" % (counter)
) << u #Save the displacements for later stress computations
epsilon += deps
t += dt
counter += 1
ID += 1
if ID != 0:
sigma -= dsig
ID = 0
######################################################################
kk += 1
#updating
ginv_i.epsilon = epsilon
elif event.type == pygame.MOUSEBUTTONDOWN:
if btn.obj.collidepoint(mouse):
countInputs=1;
u1 = inputbox.ask(DISPLAYSURF, "Initial velocity, u",countInputs)
u2 = float (u1)
countInputs=countInputs+5;
a1 = inputbox.ask(DISPLAYSURF, "Acceleration, a",countInputs)
a2 = float (a1)
countInputs=countInputs+5;
t1 = inputbox.ask(DISPLAYSURF, "Time, t",countInputs)
t2 = float (t1)
v1 = u2 + a2 * t2
v2 = str (v1)
font = pygame.font.Font(None, 36)
text = font.render("The Final Velocity hence calculated is "+v2+"metre/sec", 1, (10, 10, 10))
animation(DISPLAYSURF,'right',text)
elif btn2.obj.collidepoint(mouse):
countInputs=1;
u1 = inputbox.ask(DISPLAYSURF, "Initial Velocity, u",countInputs)
u2 = float (u1)
countInputs=countInputs+5;
a1 = inputbox.ask(DISPLAYSURF, "Acceleration, a",countInputs)
a2 = float (a1)
countInputs=countInputs+5;
v1 = inputbox.ask(DISPLAYSURF, "Final Velocity, t",countInputs)
v2 = float (v1)
t1 = (v2-u2)/a2
t2 = str (t1)
if(t1<0):
errorScreen(DISPLAYSURF,"Invalid datas entered")
font = pygame.font.Font(None, 36)
def __init__(self, mapSize, parent, repr):
QtGui.QGraphicsItemGroup.__init__(self)
#Attributes
self.__mapSize = mapSize
self.__parent = parent
self.__health = 100
self.__repr = repr
self.__gunLock = "free"
self.__radarLock = "Free"
#animation
self.__runAnimation = animation("run")
self.__targetAnimation = animation("target")
self.__physics = physics(self.__runAnimation)
#graphics
self.maskColor = QtGui.QColor(0, 255, 255)
self.gunMaskColor = QtGui.QColor(0, 255, 255)
self.radarMaskColor = QtGui.QColor(0, 255, 255)
#load img
self.__base = QtGui.QGraphicsPixmapItem()
self.__base.pixmap = QtGui.QPixmap(os.getcwd() + "/robotImages/baseGrey.png")
self.__base.setPixmap(self.__base.pixmap)
self.addToGroup(self.__base)
self.__baseWidth = self.__base.boundingRect().width()
self.__baseHeight = self.__base.boundingRect().height()
#load gun img
self.__gun = QtGui.QGraphicsPixmapItem()
self.__gun.pixmap = QtGui.QPixmap(os.getcwd() + "/robotImages/gunGrey.png")
self.__gun.setPixmap(self.__gun.pixmap)
self.addToGroup(self.__gun)
self.__gunWidth = self.__gun.boundingRect().width()
self.__gunHeight = self.__gun.boundingRect().height()
#gun position
x = self.__base.boundingRect().center().x()
y = self.__base.boundingRect().center().y()
self.__gun.setPos(x - self.__gunWidth/2.0 , y - self.__gunHeight /2.0)
#load radar img
self.__radar = QtGui.QGraphicsPixmapItem()
self.__radar.pixmap = QtGui.QPixmap(os.getcwd() + "/robotImages/radar.png")
self.__radar.setPixmap(self.__radar.pixmap)
self.addToGroup(self.__radar)
self.__radarWidth = self.__radar.boundingRect().width()
self.__radarHeight = self.__radar.boundingRect().height()
#radar position
self.__radar.setPos(x - self.__radarWidth/2.0 , y - self.__radarHeight /2.0)
#load radarField
firstPoint = QtCore.QPointF(x - self.__radarWidth/2, y)
secondPoint = QtCore.QPointF(x + self.__radarWidth/2, y)
thirdPoint = QtCore.QPointF(x + 4*self.__radarWidth, y + 700)
fourthPoint = QtCore.QPointF(x - 4*self.__radarWidth, y+ 700)
qPointListe = []
qPointListe.append(firstPoint)
qPointListe.append(secondPoint)
qPointListe.append(thirdPoint)
qPointListe.append(fourthPoint)
self.__radarField = radarField(qPointListe, self, "poly")
#__largeRadarField
qPointListe.remove(fourthPoint)
qPointListe.remove(thirdPoint)
thirdPoint = QtCore.QPointF(x + 10*self.__radarWidth, y + 400)
fourthPoint = QtCore.QPointF(x - 10*self.__radarWidth, y+ 400)
qPointListe.append(thirdPoint)
qPointListe.append(fourthPoint)
self.__largeRadarField = radarField(qPointListe, self, "poly")
#thinRadarField
qPointListe.remove(fourthPoint)
qPointListe.remove(thirdPoint)
thirdPoint = QtCore.QPointF(x + 0.4*self.__radarWidth, y + 900)
fourthPoint = QtCore.QPointF(x - 0.4*self.__radarWidth, y+ 900)
qPointListe.append(thirdPoint)
qPointListe.append(fourthPoint)
self.__thinRadarField = radarField(qPointListe, self, "poly")
#roundRadarField
self.__roundRadarField = radarField([0, 0, 300, 300], self, "round")
self.addToGroup(self.__roundRadarField)
self.__roundRadarField.setPos(x - self.__roundRadarField.boundingRect().width()/2.0 , y - self.__roundRadarField.boundingRect().height() /2.0)
#add to group
self.addToGroup(self.__radarField)
self.addToGroup(self.__largeRadarField)
self.addToGroup(self.__thinRadarField)
self.__largeRadarField.hide()
self.__thinRadarField.hide()
self.__roundRadarField.hide()
#Set the bot color in RGB
self.setColor(0, 200, 100)
self.setGunColor(0, 200, 100)
self.setRadarColor(0, 200, 100)
self.setBulletsColor(0, 200, 100)
#set the Origin point for Transformation:
#radarField
self.__radarField.setTransformOriginPoint(x, y)
self.__largeRadarField.setTransformOriginPoint(x, y)
self.__thinRadarField.setTransformOriginPoint(x, y)
#base
x = self.__baseWidth/2
y = self.__baseHeight/2
self.__base.setTransformOriginPoint(x, y)
#gun
x = self.__gunWidth/2
y = self.__gunHeight /2
self.__gun.setTransformOriginPoint(x, y)
#radar
x = self.__radarWidth/2
y = self.__radarHeight /2
self.__radar.setTransformOriginPoint(x, y)
#add self items in items to avoid collisions
self.__items = set([self, self.__base, self.__gun, self.__radar, self.__radarField, self.__largeRadarField, self.__thinRadarField, self.__roundRadarField])
#init the subclassed Bot
self.init()
self.__currentAnimation = []
solver_parameters={
"newton_solver": {
"relative_tolerance": 9e-10,
"absolute_tolerance": 9e-10,
"maximum_iterations": 80
}
})
######################################################################
#making sure we start saving results after the pressure is imposed (if there is any pressure). Also making sure we save results every 10 steps
if sigma <= sigmax:
sigma += dsig
if sigma > sigmax:
#Save animation every 10 steps to save space and time
if counter % 10 == 0:
animation(kk, u, t, vtkfile1, vtkfile2, vtkfile3, vtkfile4,
vtkfile5)
######################################################################
#Save the displacements for later stress computations
File("Circumferential/displacement/u%d.xml" % (counter)) << project(
u, V) #Save the displacements for later process
######################################################################
#Update loop info
epsilon += deps
counter += 1
t += dt
ID += 1
######################################################################
######################################################################
alpha_z = torque / I_b
omega_z += alpha_z * dt
theta_z += omega_z * dt
alpha_t = -torque / I_t
omega_t += alpha_t * dt
theta_t += omega_t * dt
acc_x = 0 + torque / (tail_w * block_mass / 2) * np.sin(theta_z +
theta_t)
acc_y = -9.81 + torque / (tail_w * block_mass /
2) * (-np.cos(theta_z + theta_t))
vel_x += acc_x * dt
vel_y += acc_y * dt
pos_x += vel_x * dt
pos_y += vel_y * dt
self.block_state = np.array([[pos_x, vel_x, acc_x],
[pos_y, vel_y, acc_y],
[theta_z, omega_z, alpha_z]])
self.tail_state = np.array([theta_t, omega_t, alpha_t])
u = [0.2, 0.4, 0.3, 0.4, 0.5, 0.3, 0.5]
if __name__ == '__main__':
C = MyController(input_torque=u, T=5)
block_traj, tail_traj = C.dynamics()
animation(block_traj, tail_traj, target_point=(0, 0, 0))
axs[0][0].plot(t, pos_x)
axs[0][0].set_ylabel('x(t)')
axs[1][0].plot(t, pos_y)
axs[1][0].set_ylabel('y(t)')
axs[2][0].plot(t, theta_block)
axs[2][0].set_ylabel('$\\theta_b$')
axs[3][0].plot(t, theta_tail)
axs[3][0].set_ylabel('$\\theta_t$')
axs[0][1].plot(t, vel_x)
axs[0][1].set_ylabel('$vel_x$')
axs[1][1].plot(t, vel_y)
axs[1][1].set_ylabel('$vel_y$')
axs[2][1].plot(t, omega_block)
axs[2][1].set_ylabel('$\omega_b(t)$')
axs[3][1].plot(t, omega_tail)
axs[3][1].set_ylabel('$\omega_t(t)$')
fig2 = plt.figure("Input Trajectory")
plt.plot(t, u)
plt.ylabel("torque")
plt.xlabel('Time (s)')
block_traj = []
tail_traj = []
for i in range(N):
block_traj.append((pos_x[i], pos_y[i], theta_block[i]))
tail_traj.append(theta_tail[i])
animation(block_traj, tail_traj, goal_state)
def __init__(self, mapSize, parent, repr):
QGraphicsItemGroup.__init__(self)
# 公有属性
self.gamePlace = 1
# 私有属性
self.__mapSize = mapSize
self.__parent = parent
self.__health = 100
self.__repr = repr
self.__gunLock = "free"
self.__radarLock = "Free" # 字面意思是雷达锁
# 两个动画对象、一个physics对象
self.__runAnimation = animation("run")
self.__targetAnimation = animation("target")
self.__physics = physics(self.__runAnimation) # 一个physics对象
# 设置一些颜色
self.maskColor = QColor(0, 255, 255)
self.gunMaskColor = QColor(0, 255, 255) # 炮膛颜色
self.radarMaskColor = QColor(0, 255, 255) # 雷达颜色
# 加载车身图片
self.__base = QGraphicsPixmapItem()
self.__base.pixmap = QPixmap(os.getcwd() + "/robotImages/baseGrey.png")
self.__base.setPixmap(self.__base.pixmap)
self.addToGroup(self.__base)
self.__baseWidth = self.__base.boundingRect().width()
self.__baseHeight = self.__base.boundingRect().height()
# 加载炮膛图片
self.__gun = QGraphicsPixmapItem()
self.__gun.pixmap = QPixmap(os.getcwd() + "/robotImages/gunGrey.png")
self.__gun.setPixmap(self.__gun.pixmap)
self.addToGroup(self.__gun)
self.__gunWidth = self.__gun.boundingRect().width()
self.__gunHeight = self.__gun.boundingRect().height()
# 炮膛位置
x = self.__base.boundingRect().center().x()
y = self.__base.boundingRect().center().y()
self.__gun.setPos(x - self.__gunWidth / 2.0,
y - self.__gunHeight / 2.0)
# --------------------------雷达初始化-----------------------------
# 加载雷达图片
self.__radar = QGraphicsPixmapItem()
self.__radar.pixmap = QPixmap(os.getcwd() + "/robotImages/radar.png")
self.__radar.setPixmap(self.__radar.pixmap)
self.addToGroup(self.__radar)
self.__radarWidth = self.__radar.boundingRect().width()
self.__radarHeight = self.__radar.boundingRect().height()
# radar position
self.__radar.setPos(x - self.__radarWidth / 2.0,
y - self.__radarHeight / 2.0)
# 加载雷达范围
firstPoint = QPointF(x - self.__radarWidth / 2, y)
secondPoint = QPointF(x + self.__radarWidth / 2, y)
thirdPoint = QPointF(x + 4 * self.__radarWidth, y + 700)
fourthPoint = QPointF(x - 4 * self.__radarWidth, y + 700)
qPointListe = []
qPointListe.append(firstPoint)
qPointListe.append(secondPoint)
qPointListe.append(thirdPoint)
qPointListe.append(fourthPoint)
self.__radarField = radarField(qPointListe, self, "poly")
# 大雷达范围
qPointListe.remove(fourthPoint)
qPointListe.remove(thirdPoint)
thirdPoint = QPointF(x + 10 * self.__radarWidth, y + 400)
fourthPoint = QPointF(x - 10 * self.__radarWidth, y + 400)
qPointListe.append(thirdPoint)
qPointListe.append(fourthPoint)
self.__largeRadarField = radarField(qPointListe, self, "poly")
# 细雷达范围
qPointListe.remove(fourthPoint)
qPointListe.remove(thirdPoint)
thirdPoint = QPointF(x + 0.4 * self.__radarWidth, y + 900)
fourthPoint = QPointF(x - 0.4 * self.__radarWidth, y + 900)
qPointListe.append(thirdPoint)
qPointListe.append(fourthPoint)
self.__thinRadarField = radarField(qPointListe, self, "poly")
# 弧形雷达范围
self.__roundRadarField = radarField([0, 0, 300, 300], self, "round")
self.addToGroup(self.__roundRadarField)
self.__roundRadarField.setPos(
x - self.__roundRadarField.boundingRect().width() / 2.0,
y - self.__roundRadarField.boundingRect().height() / 2.0)
# add to group
self.addToGroup(self.__radarField)
self.addToGroup(self.__largeRadarField)
self.addToGroup(self.__thinRadarField)
# 一个坦克上有三个雷达范围，但初始化的时候都是隐藏的
self.__largeRadarField.hide()
self.__thinRadarField.hide()
self.__roundRadarField.hide()
# --------------------------雷达初始化（上-----------------------------
# 设置坦克颜色：RGB
self.setColor(0, 200, 100)
self.setGunColor(0, 200, 100)
self.setRadarColor(0, 200, 100)
self.setBulletsColor(0, 200, 100)
# 设置原点:
# 雷达范围
self.__radarField.setTransformOriginPoint(x, y)
self.__largeRadarField.setTransformOriginPoint(x, y)
self.__thinRadarField.setTransformOriginPoint(x, y)
# 车身
x = self.__baseWidth / 2
y = self.__baseHeight / 2
self.__base.setTransformOriginPoint(x, y)
# 炮膛
x = self.__gunWidth / 2
y = self.__gunHeight / 2
self.__gun.setTransformOriginPoint(x, y)
# 雷达（雷达与雷达范围不是一个东西
x = self.__radarWidth / 2
y = self.__radarHeight / 2
self.__radar.setTransformOriginPoint(x, y)
# 添加self.item到items中以防冲突
self.__items = set([
self, self.__base, self.__gun, self.__radar, self.__radarField,
self.__largeRadarField, self.__thinRadarField,
self.__roundRadarField
])
# 初始化基类(初始化子类中的初始化函数
self.init()
# 当前动画
self.__currentAnimation = []
dimension = 10
bounds = -5, 5
beta = .5
pr = .7
tournament = 5
w = .5
c1 = .5
c2 = 1
pa = .25
dp = .1
def ge():
X = np.array([src.solution(my_func, dimension, bounds) for i in range(n)])
[Xi.initRandom() for Xi in X]
src.solution.updateHistory(X)
for it in range(iteration):
X1 = src.op.op_de(X, src.op.select_random, src.op.mut_de,
src.op.crx_exponential)
X = src.op.replace_if_random(X, X1)
X1 = src.op.op_pso(X, src.op.select_random, src.op.mut_pso,
src.op.crx_exponential)
X = X1
src.solution.updateHistory(X)
return X
ge()
animation(src.solution.history, my_func, *bounds)
#print(src.solution.best.getFitness())
from a_star import a_star
from robots import diff_drive, extended_bicycle, front_wheel_drive, simple_bicycle
from RRT import RRT
from RRT_star import RRT_star
from kalman_filter import Kalman_filter
from manual_control import ManualControl
from mpc import NonlinearMPC
import cubic_spline_planner
if __name__ == '__main__':
# Initialize simulation parameters
plt.close("all")
time = 0.0
dT = 0.1
anime = animation()
play_animation = True
###################### Initialize map ########################
resolution = 0.1 # map resolution
map_limits = [-20, 20, -20, 20] # [min_x, max_x, min_y, max_y]
obstacles = ([2, 8, 5], [-2, 3, 6], [9, -4, 7])
# obstacles = () # empty map
inflation = 3.0
maps = maps(resolution, map_limits, obstacles, inflation)
grid = maps.make_grid() # get grid map with inflated obstacles
############### Initialize controls and states ###############
x = np.random.randint(-20, 0) # todo colission check for intial states
y = np.random.randint(-20, 0)
theta = np.random.randint(-np.pi / 2 * 10.0, np.pi / 2 * 10.0) / 10.0
# states = np.array([[x],
2.74953362e+00 ,2.71990461e+00 ,2.69026044e+00 ,2.66059470e+00,
2.63090104e+00 ,2.60117339e+00 ,2.57140620e+00 ,2.54159396e+00,
2.51173135e+00 ,2.48181360e+00 ,2.45183618e+00 ,2.42179482e+00,
2.39168578e+00 ,2.36150552e+00 ,2.33125110e+00 ,2.30091997e+00,
2.27050996e+00 ,2.24001952e+00 ,2.20944737e+00 ,2.17879291e+00,
2.14805614e+00 ,2.11723759e+00 ,2.08633843e+00 ,2.05536048e+00,
2.02430608e+00 ,1.99317852e+00 ,1.96198143e+00 ,1.93071956e+00,
1.89939828e+00 ,1.86802374e+00 ,1.83660294e+00 ,1.80514381e+00,
1.77365510e+00 ,1.74214617e+00 ,1.71062767e+00 ,1.67911086e+00,
1.64760821e+00 ,1.61613270e+00 ,1.58469856e+00 ,1.55332077e+00,
1.52201528e+00 ,1.49079901e+00 ,1.45968964e+00 ,1.42870582e+00,
1.39786702e+00 ,1.36719367e+00 ,1.33670668e+00 ,1.30642800e+00,
1.27638026e+00 ,1.24658670e+00 ,1.21707128e+00 ,1.18785834e+00,
1.15897307e+00 ,1.13044076e+00 ,1.10228739e+00 ,1.07453897e+00,
1.04722198e+00 ,1.02036297e+00 ,9.93988635e-01 ,9.68125547e-01,
9.42800298e-01 ,9.18039245e-01 ,8.93868544e-01 ,8.70313817e-01,
8.47400466e-01 ,8.25153163e-01 ,8.03595922e-01 ,7.82752083e-01,
7.62644216e-01 ,7.43293900e-01 ,7.24721762e-01 ,7.06947291e-01,
6.89988734e-01 ,6.73863373e-01 ,6.58586835e-01 ,6.44173445e-01,
6.30636179e-01 ,6.17986357e-01 ,6.06233978e-01 ,5.95386861e-01,
5.85451690e-01 ,5.76433251e-01 ,5.68334386e-01 ,5.61156407e-01,
5.54898637e-01 ,5.49558600e-01 ,5.45131778e-01 ,5.41611772e-01,
5.38990505e-01 ,5.37257382e-01 ,5.36400000e-01 ,5.36400000e-01]
body_traj = []
for i in range(len(x)):
body_traj.append((x[i], y[i], theta_body[i]))
animation(body_traj, theta_tail, [0,0,0])
-1.85725436e+00, -1.81558722e+00, -1.77386453e+00, -1.73209498e+00,
-1.69028780e+00, -1.64845276e+00, -1.60660019e+00, -1.56474095e+00,
-1.52288642e+00, -1.48104851e+00, -1.43923963e+00, -1.39747271e+00,
-1.35576109e+00, -1.31411863e+00, -1.27255962e+00, -1.23109873e+00,
-1.18975106e+00, -1.14853205e+00, -1.10745755e+00, -1.06654368e+00,
-1.02580684e+00, -9.85263721e-01, -9.44931254e-01, -9.04826520e-01,
-8.64966799e-01, -8.25369495e-01, -7.86052112e-01, -7.47032210e-01,
-7.08327383e-01, -6.69955247e-01, -6.31933356e-01, -5.94279215e-01,
-5.57010209e-01, -5.20143613e-01, -4.83696527e-01, -4.47685859e-01,
-4.12128282e-01, -3.77040236e-01, -3.42437846e-01, -3.08336991e-01,
-2.74753165e-01, -2.41701550e-01, -2.09196943e-01, -1.77253765e-01,
-1.45886015e-01, -1.15107274e-01, -8.49306919e-02, -5.53689476e-02,
-2.64342795e-02, 1.86155531e-03, 2.95072643e-02, 5.64920737e-02,
8.28057120e-02, 1.08438294e-01, 1.33380564e-01, 1.57623722e-01,
1.81159450e-01, 2.03979913e-01, 2.26077820e-01, 2.47446360e-01,
2.68079189e-01, 2.87970472e-01, 3.07114848e-01, 3.25507438e-01,
3.43143823e-01, 3.60020057e-01, 3.76132628e-01, 3.91478504e-01,
4.06055101e-01, 4.19860261e-01, 4.32892298e-01, 4.45149941e-01,
4.56632267e-01, 4.67338819e-01, 4.77269494e-01, 4.86424635e-01,
4.94804933e-01, 5.02411478e-01, 5.09245682e-01, 5.15309275e-01,
5.20604402e-01, 5.25133536e-01, 5.28899499e-01, 5.31905448e-01,
5.34154888e-01, 5.35651657e-01, 5.36400000e-01, 5.36400000e-01]
body_traj = []
for i in range(len(x)):
body_traj.append((x[i], y[i], theta_body[i]))
target_point = (body_traj[-1][0], body_traj[-1][1])
animation(body_traj, theta_tail, target_point)
