def update(self, value):
"""Periodic update function"""
# Set a callback for 20ms
glutTimerFunc(20, self.update, 0)
# Change things here
for a in self.asteroids:
a.update()
for enemy in self.enemies:
enemy.update()
self.ship.update()
self.collision()
particle.update()
self.game_update()
self._level_frame += 1
if self._level_frame % 500 == 0:
newalien = levels.level[self.level].enter_alien(self.ship)
if newalien:
self.enemies.add(newalien)
# Cause a re-display
glutPostRedisplay()
def draw_particles(self):
""" Updating particle kinematics and displaying particles on screen """
for particle in self.particles:
particle.update()
particle.follow(self.flowfield)
pygame.draw.rect(self.display, colour, (int(particle.pos.x), int(particle.pos.y), particle_size, particle_size))
if not draw_flow_field:
pygame.draw.aaline(self.display, colour, (particle.pos.x, particle.pos.y), (particle.prev_pos.x, particle.prev_pos.y), 1)
def draw_particles(self):
""" Updating particle kinematics and displaying particles on screen """
for particle in self.particles:
particle.update()
particle.follow(self.flowfield)
if show_particles:
pygame.draw.rect(self.display, "white",
(int(particle.pos.x), int(particle.pos.y),
particle_radius, particle_radius))
def update(self, dt):
toremove = []
for particle in self.particles:
particle.update(dt)
if particle.dead:
toremove.append(particle)
for delete in toremove:
self.particles.remove(delete)
if len(self.particles) > self.limit:
self.popparticle()
def update(self, value):
if self.level < 6 and self.ship.lives > -1:
glutTimerFunc(20, self.update, 0)
for a in self.asteroids:
a.update()
for enemy in self.enemies:
enemy.update()
self.ship.update()
self.collision()
particle.update()
self.game_update()
self._level_frame += 1
if self._level_frame % 500 == 0:
newalien = levels.level[self.level].enter_alien(self.ship)
if newalien:
self.enemies.add(newalien)
glutPostRedisplay()
def draw_particles(self):
"""
"""
for particle in self.particles:
self.window.blit(particle.update(), particle.pos)
for i in xrange(len(self.particles)):
if self.particles[i].dead:
del self.particles[i]
break
def draw_particles(self):
"""
"""
for particle in self.particles:
self.window.blit(particle.update(), particle.pos)
for i in xrange(len(self.particles)):
if self.particles[i].dead:
del self.particles[i]
break
ax.set_xlim(0, int(BoxL))
ax.set_ylim(0, int(BoxL))
camera = Camera(fig)
nframes = 1
# model step (main loop)
for t in range(spf * frames):
# interact pari-wise
for a, b in interacting_pairs(ptcls, idxs, cut_off, BoxL):
interact(ptcls[a], ptcls[b], lam, BoxL, dt)
# update array of rods
ptcls = np.array(
[update(ptcl, sigma, alpha, dt, BoxL, D_T, D_R, rat_gam) for ptcl in ptcls]
)
# take snapshots for animation
if np.mod(t, spf) == 0:
if args.CoM:
if args.color_orientation:
ax.scatter(
[ptcl.cmx for ptcl in ptcls],
[ptcl.cmy for ptcl in ptcls],
s=psize,
c=[np.mod(ptcl.theta, np.pi) for ptcl in ptcls],
vmin=0,
vmax=np.pi,
cmap=cmap_orientation,
marker="o",
def main():
# Create a random points to interpolate into a path
x = np.arange(0, 30, 1) # x coord
x = np.append(x, x[::-1])
y = np.ones(len(x))
# designing course
# TODO: function to create courses
y = [x * 2 for x in x[:15]]
y.extend([x * 3 for x in x[15:30]])
y.extend([x * 1.2 for x in x[30:55]])
y.extend([x % 5 for x in x[55:60]])
y[-5:] = [-1, -1, -1, -1, -2]
# prepare map for func
map_ = np.vstack((x, y))
# draw map,landmarks & get trajectory
path = make_map(map_, landmarks=5, random_seed=42)
lmark_pos = np.array(make_map.landmarks)
# round path for controller
x_path = [round(x, 4) for x in path[0][:]]
y_path = [round(y, 4) for y in path[1][:]]
# state and PID object
state = State(x=x_path[0], y=y_path[0], yaw=np.radians(90.0), v=0.0)
pd = PID(Kp=0.5, Ki=0.1, Kd=-0.15)
target_speed = 30.0 / 3.6 # km/h - > [m/s]
# Lists to store
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
lat_error = [0.0]
# Setup Initial values
target_index, _ = calc_target_index(state, x_path, y_path)
last_index = len(x_path) - 1
max_sim_time = 50.0
dt = 0.1
time = 0.0
show_animation = True
# setup particle filter
N = 200
particles = gaussian_particles([0, 0, 0], [1, 1, 1], N)
weights = np.ones(N) / N # equal weight to all particles
position = np.array([state.x, state.y, state.yaw])
xs = []
NL = 5
while time <= max_sim_time and last_index > target_index:
# distance & heading to landmark
#zs1 = distance_to(lmark_pos,position,0.1,0.1)
zs = (np.linalg.norm(lmark_pos - [position[0], position[1]], axis=1) +
(np.random.randn(NL) * 0.1))
#print("Zs:{}".format(zs[:5]))
# set up controls
ai = pd.pid_control(target_speed, state.v, lat_error[-1], time)
di, target_index = stanley(state, x_path, y_path, target_index)
state.update(ai, di)
# predict where particles goin
particles = predict_2(particles, (ai, di), state.v, std=(2, 2), dt=dt)
# combine with measurements
weights = update(particles, weights, z=zs, R=1, landmarks=lmark_pos)
# check Effective N
if neff(weights) < N / 2:
indexes = stratified_resample(weights)
particles, weights = resample_from_index(particles, weights,
indexes)
assert np.allclose(weights, 1 / N)
mu, _ = estimate(particles, weights)
xs.append(mu)
time += dt
# store data for plotting
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
lat_error.append(stanley.lat_error)
position = np.array([state.x, state.y, state.yaw])
# speed up time if oscilaltions
if stanley.lat_error > abs(1.5):
time += 1
if show_animation:
Particle = [particles, mu]
simple_animation(path, [x, y],
lmark_pos,
time,
max_sim_time,
Particle=Particle,
plot_particles=True)
assert last_index >= target_index, "Cannot reach goal"
if show_animation:
#plt.plot(path[0][:],path[1][:], ".r", label = 'course')
#plt.plot(x,y,'-b',label='trajectory')
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.ylim((-5, 80))
plt.xlim((-45, 80))
plt.grid(True)
_, (ax1, ax2) = plt.subplots(2, sharex='row')
ax1.plot(t, [iv * 3.6 for iv in v], "-r")
ax1.set_ylabel("Speed[km/h]")
ax1.grid(True)
ax2.plot(t, lat_error, label='lateral error to next [m]')
ax2.set_ylabel("Lateral Error")
ax2.set_xlabel("Time[s]")
plt.grid(True)
plt.show()
def updateParticles(self):
for particle in self.active_particles:
particle.update(self)
if particle.active == False:
self.active_particles.remove(particle)
def draw_particles(self):
""" Updating particle kinematics and displaying particles on screen """
for particle in self.particles:
particle.update()
particle.follow(self.flowfield)
pygame.draw.rect(self.display, (0,0,0), (int(particle.pos.x), int(particle.pos.y), 2, 2))
