def mpc_problem(init_phi, ts, ref_phis, pred_horizon): """ uses MPC to track the reference phis, using a stepsize and a predictive horizon. ts should be separated by stepsize. """ # set up the system it gets applied to y0mpc = pmodel.lc(init_phi * pmodel.T / (2 * np.pi) + start_time) # get step size, current phase, etc stepsize = ts[1] - ts[0] u_input = [] sys_state = y0mpc sys_phis = [] for idx, inst_time in enumerate(ts): #get the ref phase at the time, compare to system phase at the time ref_phi = ref_phis[idx] # remember, phi0 is defined as when the 0-cross happens sys_phi = (pmodel.phase_of_point(sys_state) - pmodel._t_to_phi(start_time)) % (2 * np.pi) sys_phis.append(sys_phi) # phase error phi_diff = np.angle(np.exp(1j * sys_phi)) - np.angle( np.exp(1j * ref_phi)) delta_phi_f = -phi_diff % ( 2 * np.pi) # the desired shift is to make up that angle if np.abs(phi_diff) > 0.1: #this value may be changed as desired # calculate the optimal inputs us_opt = mpc_pred_horiz(sys_phi, delta_phi_f, stepsize, pred_horizon) u_apply = us_opt[0] else: u_apply = 0 print delta_phi_f, u_apply # move forward a step u_input.append(u_apply) mpc_param = np.copy(param) mpc_param[15] = mpc_param[15] - u_apply mpc_sys = lco.Oscillator(model(), mpc_param, sys_state) sys_progress = mpc_sys.int_odes(stepsize) sys_state = sys_progress[-1] return ts, sys_phis, u_input
def solutions_from_us(us, stepsize, init_phi): """ takes a set of us, a stepsize, and a phi0; returns the trajectories of the solutions """ pred_horiz = len(us) phases = [] ref_phases = [] states = [] times = [] running_time = 0 phi0 = init_phi y0mpc = pmodel.lc(phi0 * pmodel.T / (2 * np.pi) + start_time) sys_state = y0mpc for u_apply in us: mpc_param = np.copy(param) mpc_param[15] = mpc_param[15] - u_apply mpc_sys = lco.Oscillator(model(), mpc_param, sys_state) sys_progress = mpc_sys.int_odes(stepsize, numsteps=10) # append new times and phases times = times + list(mpc_sys.ts[:-1] + running_time) phases = phases + [ pmodel.phase_of_point(state) for state in sys_progress[:-1] ] #update for next step sys_state = sys_progress[-1] running_time = running_time + mpc_sys.ts[-1] u0_phases = init_phi + np.asarray(times) * 2 * np.pi / pmodel.T return { 'u0_phases': np.asarray(u0_phases), 'times': np.asarray(times), 'phases': np.asarray(phases), 'us': np.asarray(us) }
import numpy as np from scipy import integrate, optimize, stats import matplotlib.pyplot as plt from matplotlib import gridspec import casadi as cs from pyswarm import pso import brewer2mpl as colorbrewer #local imports from LocalModels.hirota2012 import model, param, y0in from LocalImports import LimitCycle as lco from LocalImports import PlotOptions as plo from LocalImports import Utilities as ut pmodel = lco.Oscillator(model(), param, y0in) pmodel.calc_y0() pmodel.corestationary() pmodel.limit_cycle() pmodel.find_prc() # start at negative prc region times = np.linspace(0, pmodel.T, 10001) roots = pmodel.pPRC_interp.splines[15].root_offset() neg_start_root = roots[0] # for when region becomes positive pos_start_root = roots[1] # for when region becomes negative umax = 0.06 # define a periodic spline for the start_time = pos_start_root prc_pos = stats.threshold(-pmodel.pPRC_interp(times + start_time)[:, 15],
def mpc_problem_fig2(shift_value): """ function for parallelization. returns the max time at which there is an error in phase greater than 0.1rad """ print str(shift_value) + ' started' shift_val = shift_value * pmodel.T / 24. prediction_window = 2 # set up times, and phases days = 7 shift_day = 1.75 times = np.linspace(0, days * 23.7, days * 1200 + 1) def phase_of_time(times): return (times * 2 * np.pi / pmodel.T) % (2 * np.pi) def shift_time(ts, shift_t=shift_day * pmodel.T, shift_val=shift_val): if np.size(ts) is 1: if ts >= shift_t: return ts + shift_val else: return ts elif np.size(ts) > 1: ts = np.copy(ts) ts[np.where(ts >= shift_t)] += shift_val return np.asarray(ts) else: print "unknown type supplied" # create the ref and uncontrolled traj unpert_p = pmodel.lc(times)[:, 0] pert_p = pmodel.lc(pmodel._phi_to_t(phase_of_time(shift_time(times))))[:, 0] # control inputs control_vectors = [pmodel.pdict['vdCn']] prcs = pmodel.pPRC_interp # set up the discretization control_dur = 23.7 / 12 # 2hr time_steps = np.arange(0, days * 23.7, 2 * 23.7 / 24) tstep_len = time_steps[1] control_inputs = np.zeros( [len(time_steps) - prediction_window, len(control_vectors) + 1]) single_step_ts = np.linspace(0, tstep_len, 100) # initialize the problem segment_ystart = y0in mpc_ts = [] mpc_ps = [] mpc_phis = [] mpc_pts = [] pred_phis = [0.] errors = [] continuous_phases = [] tphases = [] for idx, time in enumerate(time_steps[:-prediction_window]): #calculate current phase of oscillator mpc_phi = None while mpc_phi is None: # this sometimes fails to converge and I do not know why try: mpc_phi = pmodel.phase_of_point(segment_ystart) except: mpc_phi = 0 print segment_ystart mpc_phis.append(mpc_phi) mpc_pts.append(segment_ystart) mpc_time_start = pmodel._phi_to_t( mpc_phi) # used for calculating steps control_inputs[idx, 0] = time # get the absolute times, external times, external phases, and predicted # phases. predicted phases are what's used for the mpc abs_times = time_steps[idx:idx + prediction_window + 1] abs_phases = phase_of_time(abs_times) ext_times = shift_time(abs_times) ext_phases = phase_of_time(ext_times) # the next predicted phases do not anticipate the shift pred_ext_times = ext_times[0] + time_steps[1:prediction_window + 1] pred_ext_phases = pmodel._t_to_phi(pred_ext_times) % (2 * np.pi) # calculate the error in phase at the current step p1 = mpc_phi p2 = ext_phases[0] diff = np.min([(p1 - p2) % (2 * np.pi), (p2 - p1) % (2 * np.pi)]) errors.append(diff**2) print idx, diff**2 # only calculate if the error matters if diff**2 > 0.01: # define the objective fcn def err(parameter_shifts, future_ext_phases, mpc_time): # assert that args are good assert len(parameter_shifts)==len(future_ext_phases), \ "length mismatch between u, phi_ext" osc_phases = np.zeros(len(parameter_shifts) + 1) osc_phases[0] = mpc_phi for i, pshift in enumerate(parameter_shifts): # next oscilltor phase = curr phase + norm prog +integr pPRC def dphidt(phi, t0): return 2 * np.pi / (pmodel.T) + pshift * prcs( pmodel._phi_to_t(phi))[:, control_vectors[0]] osc_phases[i + 1] = integrate.odeint( dphidt, osc_phases[i], single_step_ts)[-1] #calc difference p1 = osc_phases[1:] p2 = future_ext_phases differences = np.asarray([(p1 - p2) % (2 * np.pi), (p2 - p1) % (2 * np.pi)]).min(0) differences = stats.threshold(differences, threshmin=0.1) #quadratic cost in time weights = (np.arange(len(differences)) + 1) return np.sum(weights * differences**2) + 0.001 * np.sum( parameter_shifts**2) # the sys functions here stop the swarm from printing its results xopt, fopt = pso(err, [-0.1] * prediction_window, [0.0] * prediction_window, args=(pred_ext_phases, mpc_time_start), maxiter=300, swarmsize=100, minstep=1e-4, minfunc=1e-4) opt_shifts = xopt opt_first_step = opt_shifts[0] else: # no action necessary opt_shifts = np.zeros(prediction_window) opt_first_step = opt_shifts[0] # simulate everything forward for one step control_inputs[idx, 1] = opt_first_step mpc_opt_param = np.copy(param) mpc_opt_param[pmodel.pdict['vdCn']] += opt_first_step mpc_model = lco.Oscillator(model(), mpc_opt_param, segment_ystart) sol = mpc_model.int_odes(tstep_len, numsteps=500) tsol = mpc_model.ts # add the step results to the overall results segment_ystart = sol[-1] mpc_ts += list(tsol + time) mpc_ps += list(sol[:, 0]) phases = [pmodel.phase_of_point(pt) for pt in sol[::60]] tphases += list(tsol[::60] + time) continuous_phases += list(phases) abs_err = np.sqrt(errors) ext_phis_output = phase_of_time(shift_time(time_steps)) simulation_results = [ control_inputs, errors, time_steps, mpc_phis, ext_phis_output, mpc_ps, mpc_ts, tphases, continuous_phases ] print str(shift_value) + ' completed' return simulation_results