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