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