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