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
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))