def optimize(rate, samples, prefix, names, mse_indices):
gates = generate_map('optimizer')
opcd = OPCD_Interface(gates['opcd_ctrl'])
bb = gates['blackbox']
vec_best = [ opcd.get(prefix + n) for n in names]
vec = vec_best
mse_min = sys.float_info.max
best_log = []
try:
for _ in range(5):
vec = map(lambda x: x * uniform(1.0 - rate, 1.0 + rate), vec)
# send new params to opcd:
for i, n in zip(range(len(names)), names):
opcd.set(prefix + n, vec[i])
# read data from autopilot and compute fitness:
mse = 0.0
for _ in range(samples):
array = loads(bb.recv())
for i in mse_indices:
mse += array[i] ** 2
mse /= samples
# evaluate mse:
if mse < mse_min:
best_log.append(mse)
opcd.persist()
mse_min = mse
vec_best = copy(vec)
else:
# we did not improve;
# use best vector as search starting point
vec = copy(vec_best)
except:
for i, n in zip(range(len(names)), names):
opcd.set(prefix + n, vec_best[i])
return best_log
def optimize(rate, samples, prefix, names, mse_indices):
gates = generate_map('optimizer')
opcd = OPCD_Interface(gates['opcd_ctrl'])
bb = gates['blackbox']
vec_best = [opcd.get(prefix + n) for n in names]
vec = vec_best
mse_min = sys.float_info.max
best_log = []
try:
for _ in range(5):
vec = map(lambda x: x * uniform(1.0 - rate, 1.0 + rate), vec)
# send new params to opcd:
for i, n in zip(range(len(names)), names):
opcd.set(prefix + n, vec[i])
# read data from autopilot and compute fitness:
mse = 0.0
for _ in range(samples):
array = loads(bb.recv())
for i in mse_indices:
mse += array[i]**2
mse /= samples
# evaluate mse:
if mse < mse_min:
best_log.append(mse)
opcd.persist()
mse_min = mse
vec_best = copy(vec)
else:
# we did not improve;
# use best vector as search starting point
vec = copy(vec_best)
except:
for i, n in zip(range(len(names)), names):
opcd.set(prefix + n, vec_best[i])
return best_log
param_names = ['kp', 'ki', 'kii', 'kd']
prefix = 'pilot.controllers.stabilizing.yaw_'
vec = [opcd.get(prefix + n) for n in param_names]
fit_best = sys.float_info.max
vec_best = vec
while True:
# apply mutation:
new = map(lambda x: x * uniform(1.0 - MUTATION_RATE, 1.0 + MUTATION_RATE),
vec)
i = choice(range(4))
vec[i] = new[
i] #map(lambda x: x * uniform(1.0 - MUTATION_RATE, 1.0 + MUTATION_RATE), vec)
# send new gains to opcd:
for i, n in zip(range(4), param_names):
opcd.set(prefix + n, vec[i])
# read data from autopilot and compute fitness:
fit = 0.0
for _ in range(NUM_SAMPLES):
array = loads(debug.recv())
gyro = float(array[3])
fit += (0.0 - gyro)**2
fit /= NUM_SAMPLES
print 'computed fitness:', fit
if fit < fit_best:
# fitness has increased, print and commit to opcd
print 'new best fitness:', vec, fit
opcd.persist()
fit_best = fit
arr[3] = arr[3][0:-1]
return arr
header = read_array()
if not header:
raise ValueError("could not read txt file header")
indices = []
for name in ['current', 'mag_cal_x', 'mag_cal_y', 'mag_cal_z']:
indices.append(header.index(name))
opcd = OPCD_Interface(generate_map('opcd_shell')['opcd_ctrl'])
data = np.loadtxt(stdin, usecols=indices)
current = np.asarray(data[:, 0])
mag_x = np.asarray(data[:, 1])
mag_y = np.asarray(data[:, 2])
mag_z = np.asarray(data[:, 3])
A = np.vstack([current, np.ones(len(current))]).T
a1, b1 = np.linalg.lstsq(A, mag_x)[0]
a2, b2 = np.linalg.lstsq(A, mag_y)[0]
a3, b3 = np.linalg.lstsq(A, mag_z)[0]
opcd.set('autopilot.cmc.scale_x', float(a1))
opcd.set('autopilot.cmc.scale_y', float(a2))
opcd.set('autopilot.cmc.scale_z', float(a3))
opcd.set('autopilot.cmc.bias', float(min(current)))
opcd.persist()
if not channels_valid:
if rt.expired():
print 'please enable your remote control'
else:
result = detectors[state].run(channels, channels_prev)
if result is not None:
if result in channel_map:
print 'channel already used, please try again'
detectors[state].reset()
else:
channel_map[state] = result
state += 1
if state == len(specs):
print channel_map
break
channels_prev = copy(channels)
sleep(0.3)
if killed:
raise Exception
print 'writing to opcd'
opcd = OPCD_Interface(socket_map['opcd_ctrl'])
for name, index, max, min in channel_map:
prefix = 'rc_cal.' + name + '.'
opcd.set(prefix + 'index', index)
opcd.set(prefix + 'max', max)
opcd.set(prefix + 'min', min)
opcd.persist()
print 'done'
except Exception:
print 'canceled by user'
Copyright (C) 2014 Tobias Simon, Ilmenau University of Technology
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. """
from sys import stdin
from scl import generate_map
from opcd_interface import OPCD_Interface
opcd = OPCD_Interface(generate_map('opcd_shell')['ctrl'])
try:
while True:
line = stdin.readline()
if not line:
break
key, val = line.split(' ')
val = float(val)
opcd.set('pilot.cal.' + key, val)
opcd.persist()
except:
print 'calibration invalid; please collect better data'
opcd = OPCD_Interface(gates['opcd_ctrl'])
debug = gates['blackbox']
param_names = ['kp', 'ki', 'kii', 'kd']
prefix = 'pilot.controllers.stabilizing.att_'
vec = [ opcd.get(prefix + n) for n in param_names]
fit_best = sys.float_info.max
vec_best = vec
while True:
# apply mutation:
vec = map(lambda x: x * uniform(1.0 - MUTATION_RATE, 1.0 + MUTATION_RATE), vec)
# send new gains to opcd:
for i, n in zip(range(4), param_names):
opcd.set(prefix + n, vec[i])
# read data from autopilot and compute fitness:
fit = 0.0
for _ in range(NUM_SAMPLES):
array = loads(debug.recv())
gyro = array[1:3]
setp = array[4:6]
fit += (gyro[0] - 0.0) ** 2 + (gyro[1] - 0.0) ** 2
fit /= NUM_SAMPLES
print 'computed fitness:', fit
if fit < fit_best:
# fitness has increased, print and commit to opcd
print 'new best fitness:', vec, fit
opcd.persist()
Copyright (C) 2014 Tobias Simon, Integrated Communication Systems Group, TU Ilmenau
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. """
from sys import stdin
from scl import generate_map
from opcd_interface import OPCD_Interface
opcd = OPCD_Interface(generate_map('opcd_shell')['opcd_ctrl'])
try:
while True:
line = stdin.readline()
if not line:
break
key, val = line.split(' ')
val = float(val)
opcd.set('autopilot.cal.' + key, val)
opcd.persist()
except:
print 'calibration invalid; please collect better data'
return arr
header = read_array()
if not header:
raise ValueError("could not read txt file header")
indices = []
for name in ['current', 'mag_cal_x', 'mag_cal_y', 'mag_cal_z']:
indices.append(header.index(name))
opcd = OPCD_Interface(generate_map('opcd_shell')['opcd_ctrl'])
data = np.loadtxt(stdin, usecols = indices)
current = np.asarray(data[:, 0])
mag_x = np.asarray(data[:, 1])
mag_y = np.asarray(data[:, 2])
mag_z = np.asarray(data[:, 3])
A = np.vstack([current, np.ones(len(current))]).T
a1, b1 = np.linalg.lstsq(A, mag_x)[0]
a2, b2 = np.linalg.lstsq(A, mag_y)[0]
a3, b3 = np.linalg.lstsq(A, mag_z)[0]
opcd.set('autopilot.cmc.scale_x', float(a1))
opcd.set('autopilot.cmc.scale_y', float(a2))
opcd.set('autopilot.cmc.scale_z', float(a3))
opcd.set('autopilot.cmc.bias', float(min(current)))
opcd.persist()
Copyright (C) 2014 Tobias Simon, Ilmenau University of Technology
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. """
from sys import stdin
from scl import generate_map
from opcd_interface import OPCD_Interface
opcd = OPCD_Interface(generate_map('opcd_shell')['ctrl'])
try:
while True:
line = stdin.readline()
if not line:
break
key, val = line.split(' ')
val = float(val)
opcd.set('pilot.cal.' + key, val)
opcd.persist()
except:
print 'calibration invalid; please collect better data'
Copyright (C) 2014 Tobias Simon, Integrated Communication Systems Group, TU Ilmenau
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. """
from sys import stdin
from scl import generate_map
from opcd_interface import OPCD_Interface
opcd = OPCD_Interface(generate_map('opcd_shell')['opcd_ctrl'])
try:
while True:
line = stdin.readline()
if not line:
break
key, val = line.split(' ')
val = float(val)
opcd.set('autopilot.cal.' + key, val)
opcd.persist()
except:
print 'calibration invalid; please collect better data'
