def foo(plant, plant_ann):
time = np.arange(0., 3.2, 0.01)
ysp = np.random.uniform(low=-1., high=1., size=len(time)) #time + 0.3
#ysp = scipy.signal.square(time)
#ysp[0] = 0
y0, u0 = [0, 0], ysp[0]
y, u = plant.sim_io(time, y0, u0, so_lti.IoCtlCst(ysp))
a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1
#_u = compute_prev_control_from_output_history_horiz(y, a0, a1, b0, b1, debug=True)
_u = compute_prev_control_from_output_history_mat(y,
a0,
a1,
b0,
b1,
h=-1,
debug=True)
ax = plt.subplot(3, 1, 1)
plt.plot(time, y, '.-')
jpu.decorate(ax, title='y', legend=True)
ax = plt.subplot(3, 1, 2)
plt.plot(time[:-1], u[:-1], '.-', label='truth')
plt.plot(time[0:-1], _u[::-1], '.-', label='est')
jpu.decorate(ax, title='u', legend=True)
err_u = _u[::-1] - u[:-1]
ax = plt.subplot(3, 1, 3)
jpu.decorate(ax, title='$\\tilde{u}$', legend=True)
plt.plot(time[:-1], err_u, '.-')
#pdb.set_trace()
plt.show()
def debug2(ctl, plant, plant_ann):
print('## Debug2')
if 0:
_input, _output = ctl.make_training_set(int(10e3))
else:
time = np.arange(0., 15.05, 0.01)
ysp = scipy.signal.square(0.3 * time)
yr, ur = ctl.ref.sim_io(time, ysp[:2], so_lti.IoCtlCst(ysp))
#cf_ctl = SoLtiIoMrcCF(ysp, yr, plant, ctl.omega_ref, ctl.xi_ref, ctl.omega_err, ctl.xi_err)
# ctl.force_weight(plant)
# test_ctl_full_ann(ctl, time, ysp, yr, ur)
#test_compute_previous_control(plant_ann, plant)
_input, _output = make_good_dataset(ctl, plant, plant_ann)
#pdb.set_trace()
if 0:
_nb = len(time) - 2
# (yr_kp1, yr_k, yr_km1) (y_k, y_km1, u_km1)
_input = [
np.zeros((_nb, 3), dtype=np.float64),
np.zeros((_nb, 3), dtype=np.float64)
]
# (y_kp1)
_output = np.zeros((_nb, 1), dtype=np.float64)
for k in range(1, len(time) - 1):
_input[0][k - 2] = [yr[k + 1], yr[k], yr[k - 1]]
_input[1][k - 2] = [yr[k], yr[k - 1], ur[k - 1]]
_output[k - 2] = yr[k + 1]
pdb.set_trace()
print('orig score {}'.format(
ctl.full_ann.evaluate(_input, _output, batch_size=32, verbose=0)))
ctl.force_weight(plant)
print('forced score {}'.format(
ctl.full_ann.evaluate(_input, _output, batch_size=32, verbose=0)))
pdb.set_trace()
def debug_ctl(omega_plant,
xi_plant,
omega_ref,
xi_ref,
omega_err,
xi_err,
plot=False):
time = np.arange(0., 6., 0.01)
plant = so_lti.IoPlantModel(omega_plant, xi_plant)
ysp = scipy.signal.square(time)
ref = so_lti.IoPlantModel(omega_ref, xi_ref)
yr, ur = ref.sim_io(time, [0, 0], so_lti.IoCtlCst(ysp))
if 0:
err_track = so_lti.IoPlantModel(omega_err, xi_err)
k0, k1, k2 = 1. / plant.a1, err_track.b1 / plant.a1, err_track.b0 / plant.a1
k3 = (plant.b1 - err_track.b1) / plant.a1
k4 = (plant.b0 - err_track.b0) / plant.a1
k5 = -plant.a0 / plant.a1
k = [k0, k1, k2, k3, k4, k5]
else:
ctl = SoLtiIoMrcCF(ysp, yr, plant, omega_ref, xi_ref, omega_err,
xi_err)
y, u = plant.sim_io(time, [1, 1], ctl)
if plot:
plot_ctl(time, ysp, yr, y)
return ctl.k
def main(
plant,
plant_ann,
omega_ref=3.,
xi_ref=0.9,
omega_err=10.,
xi_err=0.7,
train_ctl=False,
force_ctl=False,
_test_ctl=True,
ctl_epochs=5,
ctl_ann_filename='/home/poine/work/homere/homere_control/data/so_lti_io_full_ann.h5',
verbose=False):
##
## Controller
##
omega_ref, xi_ref, omega_err, xi_err = 3, 0.9, 10, 0.7
ctl = SoLtiIoMrc(plant_ann.plant_ann, omega_ref, xi_ref, omega_err, xi_err)
if train_ctl or not os.path.isfile(ctl_ann_filename):
_ann_in, _ann_out = ctl.train(ctl_ann_filename,
epochs=ctl_epochs,
dt=0.01,
_nb=int(10e3))
plot_training(ctl)
plot_training_dataset(_ann_in, _ann_out)
report_ctl_id(plant, ctl)
else:
ctl.load(ctl_ann_filename)
if force_ctl:
ctl.force_weight(plant)
#report_ctl_id(plant, ctl)
#debug2(ctl, plant, plant_ann)
if 0:
time = np.arange(0, 100, plant.dt)
ysp = scipy.signal.square(time)
yr, ur = plant.sim_io(time, [0, 0], so_lti.IoCtlCst(ysp))
ctl.yr = yr
y, u = plant.sim_io(time, [1, 1], ctl)
plot_ctl(time, ysp, yr, y)
if _test_ctl:
#time = np.arange(0, 100, plant.dt)
#ysp = scipy.signal.square(time)
#yr, ur = plant.sim_io(time, [0, 0], so_lti.IoCtlCst(ysp))
#ctl = SoLtiIoMrcCF(ysp, yr, plant, omega_ref=3., xi_ref=0.9, omega_err=10., xi_err=0.7)
test_ctl(plant, ctl)
return ctl
def prepare_io_time(self, plant):
# Record a trajectory of the real plant with a random input
time = np.arange(0, 100, plant.dt)
ysp = np.random.uniform(low=-1., high=1., size=len(time))
y, u = plant.sim_io(time, [0, 0], so_lti.IoCtlCst(ysp))
_ann_out = y[self.delay:]
_ann_in = np.zeros((len(y) - self.delay, self.input_size))
for i in range(self.delay, len(y)):
_ann_in[i - self.delay, self.y_k] = y[i - 1]
_ann_in[i - self.delay, self.y_km1] = y[i - 2]
_ann_in[i - self.delay, self.u_km1] = u[i - 2]
_ann_in[i - self.delay, self.u_k] = u[i - 1]
return _ann_in, _ann_out
def test_ctl_full_ann(ctl, time, ysp):
yr, ur = ctl.ref.sim_io(time, ysp[:2], so_lti.IoCtlCst(ysp))
y, u = np.zeros(len(time)), np.zeros(len(time))
y[0:2] = [1.2, 1.2]
_nb = len(time)
_input = [
np.zeros((_nb, 3), dtype=np.float64),
np.zeros((_nb, 3), dtype=np.float64)
]
_output = np.zeros((_nb, 2), dtype=np.float64)
for k in range(1, len(time) - 1):
_input[0][k] = np.array([[yr[k + 1], yr[k], yr[k - 1]]])
_input[1][k] = np.array([[y[k], y[k - 1], u[k - 1]]])
y[k + 1], u[k] = _output[k] = ctl.full_ann_with_ctl_out.predict(
[_input[0][k].reshape((1, 3)), _input[1][k].reshape((1, 3))])
pmrc.plot_ctl(time, ysp, yr, y)
return [_input[0][1:], _input[1][1:]], _output[1:, 0][:, np.newaxis]
def test_compute_previous_control(plant, plant_ann):
time = np.arange(0., 1.05, 0.01)
#ysp = scipy.signal.square(time)
#ysp = np.sin(time+1)
ysp = np.random.uniform(low=-1., high=1., size=len(time))
y, u = plant.sim_io(time, [1, 1], ysp[0], so_lti.IoCtlCst(ysp))
u[0] = u[1]
# test getting u_km1 from y_kp1, y_k, y_km1...y0
a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1
_u1, _u2 = np.zeros(len(time)), np.zeros(len(time))
horiz = 11
for k in range(1, len(time) - 1):
_u1[k - 1] = compute_prev_control_from_output_history(
y[:k + 1], a0, a1, b0, b1)
#_u2[k-1] = compute_prev_control_from_output_history_horiz(y[:k+1], a0, a1, b0, b1, h=horiz)#, u1=u[:k])
_u2[k - 1] = compute_prev_control_from_output_history_mat(
y[:k + 1], a0, a1, b0, b1, h=horiz) #, u1=u[:k])
_u1[-1] = _u2[-1] = u[-1]
#pdb.set_trace()
ax = plt.gca()
err_u1 = _u1 - u
err_u2 = _u2 - u
plt.hist(err_u1[1:-3], label='u1', alpha=0.5)
plt.hist(err_u2[1:-3], label='u2', alpha=0.5)
jpu.decorate(ax, title='hist', legend=True)
plt.figure()
ax = plt.subplot(3, 1, 1)
plt.plot(time, y, label='plant')
jpu.decorate(ax, title='$y$')
ax = plt.subplot(3, 1, 2)
plt.plot(time[:-2], u[:-2], label='$u real$')
plt.plot(time[:-2], _u1[:-2], label='$\\tilde{u}1$')
plt.plot(time[:-2], _u2[:-2], label='$\\tilde{u}2$')
jpu.decorate(ax, title='$u$', legend=True)
ax = plt.subplot(3, 1, 3)
plt.plot(time[1:-3], err_u1[1:-3], label='u1')
plt.plot(time[1:-3], err_u2[1:-3], label='u2')
jpu.decorate(ax, title='$u-\\tilde{u}$', legend=True)
def main(
omega_plant=1.,
xi_plant=0.5,
):
plant = so_lti.IoPlantModel(omega_plant, xi_plant)
a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1
print(plant.tf_disc)
if 0:
time = np.arange(0., 20.05, 0.01)
ysp = scipy.signal.square(0.3 * time)
y0, u0 = np.random.uniform(low=-1., high=1.,
size=2), np.random.uniform(low=-1.,
high=1.,
size=1)
y0[1] = y0[0] + np.random.uniform(low=-0.1, high=0.1, size=1)
y, u = plant.sim_io(time, y0, u0, so_lti.IoCtlCst(ysp))
plot(time, y, u, ysp)
plt.show()
horiz = 2
# build network
net_i = keras.layers.Input((horiz + 2, ),
name="net_i") #y_k, y_km1, ..., y_kmh, u_kmh
w0 = 1 / a1 * np.array([[b0], [b1], [1.], [-a0]]) + np.random.uniform(
low=-100., high=100., size=(4, 1))
pdb.set_trace()
net_l = keras.layers.Dense(
1,
activation='linear',
#kernel_initializer='uniform',
kernel_initializer=keras.initializers.Constant(w0),
input_shape=(horiz + 1, ),
use_bias=False,
name="net_l")
net_o = net_l(net_i)
_ann = keras.models.Model(inputs=net_i, outputs=net_o)
_opt = keras.optimizers.Adam(lr=0.001,
beta_1=0.9,
beta_2=0.999,
epsilon=None,
decay=0.0,
amsgrad=False)
#_opt = keras.optimizers.SGD(lr=0.01, decay=1e-6, momentum=0.9, nesterov=True)
#_ann.compile(loss='mean_squared_error', optimizer=_opt)
#_ann.compile(loss='mean_squared_logarithmic_error', optimizer=_opt)
_ann.compile(loss='mean_absolute_error', optimizer=_opt)
# prepare dataset
if 1:
ds_size = int(1e5)
time = np.arange(0., (horiz + 1) * plant.dt, plant.dt)
_input, _output = np.zeros(
(ds_size, horiz + 2), dtype=np.float64), np.zeros((ds_size, 1),
dtype=np.float64)
_us = np.zeros((ds_size, horiz))
for i in range(ds_size):
ysp = np.random.uniform(low=-1., high=1., size=horiz + 1)
y0, u0 = np.random.uniform(low=-1., high=1.,
size=2), np.random.uniform(low=-1.,
high=1.,
size=1)
y0[1] = y0[0] + np.random.uniform(low=-0.01, high=0.01, size=1)
y, u = plant.sim_io(time, y0, u0, so_lti.IoCtlCst(ysp))
_input[i, :-1], _input[i, -1] = y, u[0]
_output[i] = u[-2]
_us[i] = u[:-1]
np.savez('/tmp/ds.npz', _input=_input, _output=_output)
else:
_data = np.load('/tmp/ds.npz')
_input, _output = _data['_input'], _data['_output']
ds_size = len(_output)
# plot dataset
if True:
#
#for i in range(ds_size):
# plt.plot(time, _input[i][:-1])
for i in range(horiz + 1):
ax = plt.subplot(2, horiz + 1, i + 1)
plt.hist(_input[:, i])
jpu.decorate(ax, title='$y_{{{}}}$'.format(i))
for i in range(horiz):
ax = plt.subplot(2, horiz + 1, i + 1 + horiz + 1)
plt.hist(_us[:, i])
jpu.decorate(ax, title='$u_{{{}}}$'.format(i))
plt.show()
# train
if True:
callbacks = [
keras.callbacks.EarlyStopping(monitor='val_loss', patience=10),
keras.callbacks.ModelCheckpoint(filepath='/tmp/foo.h5',
monitor='val_loss',
save_best_only=True)
]
history = _ann.fit(_input,
_output,
epochs=1000,
batch_size=16,
verbose=1,
shuffle=True,
validation_split=0.2,
callbacks=callbacks)
plt.figure()
plt.plot(history.history['loss'])
plt.plot(history.history['val_loss'])
jpu.decorate(plt.gca(),
'loss',
xlab='epochs',
legend=['training', 'validation'])
print(' trained weights:\n{}'.format(net_l.get_weights()))
# force
if False:
print(' orig weight:\n{}'.format(net_l.get_weights()))
a0, a1, b0, b1 = plant.a0, plant.a1, plant.b0, plant.b1
net_l.set_weights([1 / a1 * np.array([[b0], [b1], [1.], [-a0]])])
print(' new weight:\n{}'.format(net_l.get_weights()))
# evaluate
res = _ann.predict(_input) - _output
loss = np.mean(np.square(res))
print('loss {:.2e}'.format(loss))
plt.figure()
plt.hist(res)
plt.show()
loss = _ann.evaluate(_input, _output)
print('loss {:.2e}'.format(loss))