def handle_collisions_inelastic(): ''' Merge particles that come closer than config.merge_threshold. Use physics of ineslastic collision. ''' n = len(particles) for i in range(n): pi = particles[i] if not pi.alive: continue for j in range(n): pj = particles[j] if not pj.alive: continue if i == j: continue d = vector.dist(pi.r, pj.r) if d < config.merge_threshold: # merge into one particle pj.alive = False pi.r = (pi.r * pi.m + pj.r * pj.m) / (pi.m + pj.m) pi.v = (pi.v * pi.m + pj.v * pj.m) / (pi.m + pj.m) pi.m += pj.m pi.q += pj.q hue = (pi.color.hsla[0] * pi.m + pj.color.hsla[0] * pj.m) / (pi.m + pj.m) pi.color.hsla = (hue, config.saturation, config.brightness, config.alpha)
def CalibrationProcess(points, norm_pipe, fit_output, accel_calibration, compass_calibration): import os if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()): print 'warning, failed to make calibration process idle, trying renice' if os.system("renice 20 %d" % os.getpid()): print 'warning, failed to renice calibration process' accel_cal = SigmaPoints(.05**2, 12, 12) compass_cal = SigmaPoints(1**2, 18, 4) #down_sigma = .05 # distance between down vectors norm = [0, 0, 1] while True: t = time.time() addedpoint = False while time.time() - t < calibration_fit_period: p = points.recv(1) if p: accel, compass, down = p if accel: accel_cal.AddPoint(accel, list(accel)) #print 'add', len(accel_cal.sigma_points) if compass and down: compass_cal.AddPoint(compass, down) addedpoint = True while True: n = norm_pipe.recv() if not n: break norm = n compass_cal.Reset() #print 'set norm', norm if not addedpoint: # don't bother to run fit if no new data continue accel_cal.RemoveOlder(10 * 60) # 10 minutes fit = FitAccel(accel_cal) if fit: # reset compass sigmapoints on accel cal dist = vector.dist(fit[0][:3], accel_calibration[:3]) if dist > .01: # only update when bias changes more than this if dist > .1: # reset compass cal from large change in accel bias compass_cal.Reset() accel_calibration = fit[0] fit_output.send( ('accel', fit, map(lambda p: p.sensor, accel_cal.sigma_points)), False) compass_cal.RemoveOlder(60 * 60) # 60 minutes fit = FitCompass(compass_cal, compass_calibration, norm) if fit: fit_output.send( ('compass', fit, map(lambda p: p.sensor + p.down, compass_cal.sigma_points)), False) compass_calibration = fit[0]
def compute_accs(): n = len(particles) for p in particles: p.a = Vector() for i in range(n): pi = particles[i] if not pi.alive: continue for j in range(n): if i == j: continue pj = particles[j] if not pj.alive: continue d = vector.dist(pi.r, pj.r) if d: a = (Particle.k_factor * pi.q * pj.q) / (d * d * pi.m) pi.a += (pi.r - pj.r) * (a / d)
def AddPoint(self, sensor, down=False): if not self.lastpoint: self.lastpoint = SigmaPoint(sensor, down) return if self.lastpoint.count < self.min_count: # require x measurements if vector.dist2(self.lastpoint.sensor, sensor) < self.sigma: self.lastpoint.add_measurement(sensor, down) return self.lastpoint = False return # use lastpoint as better sample sensor, down = self.lastpoint.sensor, self.lastpoint.down self.lastpoint = False ind = 0 for point in self.sigma_points: if point.count > 100: continue if vector.dist2(point.sensor, sensor) < self.sigma: point.add_measurement(sensor, down) if ind > 0: # move toward front of list to speed up future tests self.sigma_points = self.sigma_points[:ind-1] + [point] + [self.sigma_points[ind-1]] + self.sigma_points[ind+1:] return ind += 1 self.updated = True index = len(self.sigma_points) p = SigmaPoint(sensor, down) if index < self.max_sigma_points: # push to front of list self.sigma_points = [p] + self.sigma_points return # replace point that is closest to other points mindi = 0 mind = 1e20 for i in range(len(self.sigma_points)): dt = time.monotonic() - self.sigma_points[i].time d = [] for j in range(len(self.sigma_points)): if i == j: continue dist = vector.dist(self.sigma_points[i].sensor, self.sigma_points[j].sensor) count = min(self.sigma_points[i].count, 100) d.append(dist) d.sort() # weight based on distance to closest 2 points and time total = (d[0]+d[1])*1/dt**.2 if total < mind: mindi = i mind = total self.sigma_points[mindi] = p
def AddPoint(self, compass, down): if not self.lastpoint: self.lastpoint = SigmaPoint(compass, down) return if vector.dist2(self.lastpoint.compass, compass) < SigmaPoints.sigma: fac = .02 for i in range(3): self.lastpoint.compass[i] = fac * compass[i] + ( 1 - fac) * self.lastpoint.compass[i] self.lastpoint.down[i] += down[i] self.lastpoint.count += 1 return if self.lastpoint.count < 3: # require 3 measurements self.lastpoint = False return compass, down = self.lastpoint.compass, self.lastpoint.down for i in range(3): down[i] /= self.lastpoint.count self.lastpoint = False ind = 0 for point in self.sigma_points: if vector.dist2(point.compass, compass) < SigmaPoints.sigma: point.add_measurement(compass, down) if ind > 0: # put at front of list to speed up future tests self.sigma_points = self.sigma_points[:ind-1] + [point] + \ [self.sigma_points[ind-1]] + self.sigma_points[ind+1:] return ind += 1 index = len(self.sigma_points) p = SigmaPoint(compass, down) if index == SigmaPoints.max_sigma_points: # replace point that is closest to other points minweighti = 0 minweight = 1e20 for i in range(len(self.sigma_points)): for j in range(len(self.sigma_points)): if i == j: continue dist = vector.dist(self.sigma_points[i].compass, self.sigma_points[j].compass) count = min(self.sigma_points[i].count, 100) dt = time.time() - self.sigma_points[i].time weight = dist * count**.2 * 1 / dt**.1 #print 'ij', i, j, dist, count, dt, weight if weight < minweight: minweighti = i minweight = weight #print 'replace', minweighti, self.sigma_points[minweighti].count, time.time() - self.sigma_points[minweighti].time self.sigma_points[minweighti] = p else: self.sigma_points.append(p)
def findFitness(self, goals): if self.goalsReached >= len(goals): distToNextGoal = 5 else: distToNextGoal = dist(goals[self.goalsReached], self.pos) distToSpawn = dist(self.spawn, self.pos) self.fitness = 0 self.fitness += 2.0 * (1.0 / (self.closestDist**2)) + 1.0 / (distToNextGoal** 2) self.fitness += self.goalsReached if self.deathBy == "LastGoal" or self.allGoalsReached: self.fitness *= 5 elif self.deathBy == "Wall": self.fitness *= 0.001 elif self.deathBy == "Circle Of Death": self.fitness *= 0 elif self.deathBy == "Time": self.fitness *= 0.5
def AddPoint(self, sensor, down): if not self.lastpoint: self.lastpoint = SigmaPoint(sensor, down) return if self.lastpoint.count < self.min_count: # require x measurements if vector.dist2(self.lastpoint.sensor, sensor) < self.sigma: self.lastpoint.add_measurement(sensor, down) return self.lastpoint = False return # use lastpoint as better sample sensor, down = self.lastpoint.sensor, self.lastpoint.down for i in range(3): down[i] /= self.lastpoint.count self.lastpoint = False ind = 0 for point in self.sigma_points: if vector.dist2(point.sensor, sensor) < self.sigma: point.add_measurement(sensor, down) if ind > 0: # put at front of list to speed up future tests self.sigma_points = self.sigma_points[:ind-1] + [point] + \ [self.sigma_points[ind-1]] + self.sigma_points[ind+1:] return ind += 1 index = len(self.sigma_points) p = SigmaPoint(sensor, down) if index == self.max_sigma_points: # replace point that is closest to other points minweighti = 0 minweight = 1e20 for i in range(len(self.sigma_points)): for j in range(len(self.sigma_points)): if i == j: continue dist = vector.dist(self.sigma_points[i].sensor, self.sigma_points[j].sensor) count = min(self.sigma_points[i].count, 100) dt = time.time() - self.sigma_points[i].time weight = dist * count**.2 * 1 / dt**.1 #print('ij', i, j, dist, count, dt, weight) if weight < minweight: minweighti = i minweight = weight #print('replace', minweighti, self.sigma_points[minweighti].count, time.time() - self.sigma_points[minweighti].time) self.sigma_points[minweighti] = p return self.sigma_points.append(p)
def CalibrationProcess(cal_pipe, client): accel_points = SigmaPoints(.05**2, 12, 10) compass_points = SigmaPoints(1.1**2, 28, 3) norm = [0, 0, 1] accel_calibration = RegisterCalibration(client, 'imu.accel', [[0, 0, 0, 1], 1]) compass_calibration = RegisterCalibration(client, 'imu.compass', [[0, 0, 0, 30, 0], [1, 1], 0]) client.watch('imu.alignmentQ') if not cal_pipe: # get these through client rather than direct pipe client.watch('imu.accel') client.watch('imu.compass') client.watch('imu.fusionQPose') def debug(name): def debug_by_name(*args): s = '' for a in args: s += str(a) + ' ' client.set('imu.'+name+'.calibration.log', s) return debug_by_name last_compass_coverage = 0 while True: t = time.monotonic() addedpoint = False down = False while time.monotonic() - t < calibration_fit_period: # receive pypilot messages msg = client.receive(1) for name in msg: value = msg[name] if name == 'imu.alignmentQ' and value: norm = quaternion.rotvecquat([0, 0, 1], value) compass_points.Reset() elif name == 'imu.accel': if value: accel_points.AddPoint(value) addedpoint = True elif name == 'imu.compass' and down: if value and down: compass_points.AddPoint(value, down) addedpoint = True elif name == 'imu.fusionQPose': if value: down = quaternion.rotvecquat([0, 0, 1], quaternion.conjugate(value)) # receive calibration data if cal_pipe: p = cal_pipe.recv() while p: if 'accel' in p: accel_points.AddPoint(p['accel']) addedpoint = True if 'compass' in p: down = quaternion.rotvecquat([0, 0, 1], quaternion.conjugate(p['fusionQPose'])) compass_points.AddPoint(p['compass'], down) addedpoint = True p = cal_pipe.recv() cals = [(accel_calibration, accel_points), (compass_calibration, compass_points)] for calibration, points in cals: calibration.age.update() if points.Updated(): calibration.sigmapoints.set(points.Points()) if not addedpoint: # don't bother to run fit if no new data continue accel_points.RemoveOlder(10*60) # 10 minutes fit = FitAccel(debug('accel'), accel_points) if fit: # reset compass sigmapoints on accel cal dist = vector.dist(fit[0][:3], accel_calibration.value[0][:3]) if dist > .01: # only update when bias changes more than this if dist > .08: # reset compass cal from large change in accel bias compass_points.Reset() debug('accel')('reset compass from large accel bias') accel_calibration.set(fit) accel_calibration.points.set(accel_points.Points()) else: debug('accel')('calibration distance too small ', dist) compass_points.RemoveOlder(20*60) # 20 minutes fit = FitCompass(debug('compass'), compass_points, compass_calibration.value[0], norm) if fit: # ignore decreasing compass coverage new_coverage = fit[1][2] if new_coverage < last_compass_coverage: debug('compass')('ignoring decreasing coverage') else: compass_calibration.set(fit) compass_calibration.points.set(compass_points.Points()) else: last_compass_coverage = 0 # reset
def CalibrationProcess(cal_pipe): import os if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()): print( 'warning, failed to make calibration process idle, trying renice') if os.system("renice 20 %d" % os.getpid()): print('warning, failed to renice calibration process') accel_cal = SigmaPoints(.05**2, 12, 10) compass_cal = SigmaPoints(1.1**2, 24, 3) accel_calibration = [0, 0, 0, 1] norm = [0, 0, 1] def on_con(client): client.watch('imu.alignmentQ') client.watch('imu.compass.calibration') while True: time.sleep(2) try: client = pypilotClient(on_con, 'localhost', autoreconnect=True) break except Exception as e: print('nmea process failed to connect pypilot', e) def debug(name): def debug_by_name(*args): s = '' for a in args: s += str(a) + ' ' if client: client.set('imu.' + name + '.calibration.log', s) return debug_by_name while True: t = time.time() addedpoint = False while time.time() - t < calibration_fit_period: # receive pypilot messages msg = client.receive() for name in msg: value = msg[name]['value'] if name == 'imu.alignmentQ' and value: norm = quaternion.rotvecquat([0, 0, 1], value) compass_cal.Reset() elif name == 'imu.compass.calibration': compass_calibration = value[0] # receive calibration data p = cal_pipe.recv(1) if p: if 'accel' in p: accel_cal.AddPoint(p['accel']) addedpoint = True if 'compass' in p: compass_cal.AddPoint(p['compass'], p['down']) addedpoint = True # send updated sigmapoints as well cals = {'accel': accel_cal, 'compass': compass_cal} for name in cals: cal = cals[name] if cal.Updated(): points = cal.Points() client.set('imu.' + name + '.calibration.sigmapoints', points) if not addedpoint: # don't bother to run fit if no new data continue accel_cal.RemoveOlder(10 * 60) # 10 minutes fit = FitAccel(debug('accel'), accel_cal) if fit: # reset compass sigmapoints on accel cal dist = vector.dist(fit[0][:3], accel_calibration[:3]) if dist > .01: # only update when bias changes more than this if dist > .08: # reset compass cal from large change in accel bias compass_cal.Reset() accel_calibration = fit[0] client.set('imu.accel.calibration', fit) compass_cal.RemoveOlder(20 * 60) # 20 minutes fit = FitCompass(debug('compass'), compass_cal, compass_calibration, norm) if fit: client.set('imu.compass.calibration', fit) compass_calibration = fit[0]
def distance_to_segment(point, a, b): if dot(b - a, point - a) <= 0 or dot(a - b, point - b) <= 0: return min(dist(point, a), dist(point, b)) return abs(cross(point - a, normalize(b - a)))
def create_obstacle_circle(center, radius): o = Obstacle(lambda r: dist(r, center) - radius, lambda r: normalize(r - center), shapes.Circle(center, radius)) o.center = center return o
def intersect_circle(self, c): if dist(self.center, c.center) <= self.radius + c.radius + 1e-5: return True else: return None