def plot_truth_vel(_ds, filename=None):
fig = jpu.prepare_fig(window_title='Truth')
ax = plt.subplot(2, 1, 1)
#plt.plot(_ds.truth_stamp, _ds.truth_norm_vel, '.')
plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 0], '.', label='x')
plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 1], '.', label='y')
plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 2], '.', label='z')
jpu.decorate(ax,
title='Truth velocity',
xlab='time in s',
ylab='m/s',
legend=True,
xlim=None,
ylim=None)
ax = plt.subplot(2, 1, 2)
plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 0], '.', label='p')
plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 1], '.', label='q')
plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 2], '.', label='r')
jpu.decorate(ax,
title='Truth rotational velocity',
xlab='time in s',
ylab='rad/s',
legend=True,
xlim=None,
ylim=[-2, 2])
jpu.savefig(filename)
def compute_derivatives(self):
self.enc_vel_lw = self.data['lw_angle'][1:] - self.data['lw_angle'][:-1]
#self.enc_vel_rw = self.enc_rw[1:] - self.enc_rw[:-1]
self.enc_dt = self.data['stamp'][1:] - self.data['stamp'][:-1]
self.enc_vel_lw /= self.enc_dt
#self.enc_vel_rw /= self.enc_dt
self.enc_vel_stamp = (self.data['stamp'][:-1] +
self.data['stamp'][1:]) / 2
fs, cutoff = 100., 2 #3.2 # sampling freq and desired cutoff (Hz)
nyq = 0.5 * fs
normal_cutoff = cutoff / nyq
order = 2
# 2.0*M_PI/TIME_CONSTANT with TIME_CONSTANT = 1
b, a = scipy.signal.butter(order,
normal_cutoff,
btype='low',
analog=False)
#self.enc_vel_f_lw = scipy.signal.filtfilt(b, a, self.enc_vel_lw)
self.enc_vel_f_lw = scipy.signal.lfilter(b, a, self.enc_vel_lw)
ax = plt.subplot(2, 1, 1)
plt.plot(self.data['stamp'], self.data['lw_angle'], label='lw_angle')
jpu.decorate(ax, title="angles", xlab='time', ylab='rad', legend=True)
ax = plt.subplot(2, 1, 2)
plt.plot(self.data['stamp'],
self.data['lw_rvel'],
alpha=0.5,
label='lw_rvel')
plt.plot(self.data['stamp'], self.data['lw_rvel_f'], label='lw_rvel_f')
plt.plot(self.enc_vel_stamp, self.enc_vel_lw, label='lw_rvel1')
plt.plot(self.enc_vel_stamp, self.enc_vel_f_lw, label='lw_rvelf1')
jpu.decorate(ax, title="rvel", xlab='time', ylab='rad/s', legend=True)
def plot_pwm(_ds, filename=None):
figsize = (20.48, 2.56)
fig = jpu.prepare_fig(window_title='PWM', figsize=figsize)
ax = plt.subplot(1, 1, 1)
plt.plot(_ds.enc_stamp, _ds.lw_pwm, '.', label="lw")
plt.plot(_ds.enc_stamp, _ds.rw_pwm, '.', label="rw")
jpu.decorate(ax, title='PWM', xlab='time in s', ylab='', legend=True)
def plot_wheel_radius(self, filename=None):
fig = jpu.prepare_fig(window_title='Wheel radius regression')
inlier_mask = self.ransac_wr.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
#plt.scatter(self.ds.enc_vel_sum, self.truth_lvel_body_1[:,0], color='gold', marker='.', label='Outliers')
plt.scatter(self.ds.enc_vel_sum[outlier_mask],
self.truth_lvel_body_1[:, 0][outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_sum[inlier_mask],
self.truth_lvel_body_1[:, 0][inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
test_enc_vel_sum = np.linspace(np.min(self.ds.enc_vel_sum),
np.max(self.ds.enc_vel_sum),
100)[:, np.newaxis]
test_vel_simple = 0.5 * test_enc_vel_sum * self.wheel_radius_simple
test_vel_ransac = 0.5 * self.ransac_wr.predict(test_enc_vel_sum)
plt.plot(test_enc_vel_sum, test_vel_simple, label='simple')
plt.plot(test_enc_vel_sum, test_vel_ransac, label='ransac')
jpu.decorate(plt.gca(),
title='wheel radius fit',
xlab='enc_avg (rad/s)',
ylab="v (m/s)",
legend=True) #, xlim=[-1, 20], ylim=[-0.1, 1.])
jpu.savefig(filename)
def plot_wheel_sep(self, filename=None):
fig = jpu.prepare_fig(window_title='Wheel sep regression')
inlier_mask = self.ransac_ws.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
plt.scatter(self.ds.enc_vel_dif[outlier_mask],
self.truth_rvel_1[:, 2][outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_dif[inlier_mask],
self.truth_rvel_1[:, 2][inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
test_enc_vel_dif = np.linspace(np.min(self.ds.enc_vel_dif),
np.max(self.ds.enc_vel_dif),
100)[:, np.newaxis]
test_rvel_simple = test_enc_vel_dif * -self.wheel_radius / self.wheel_sep_simple
#test_rvel_ransac = self.ransac_wr.predict(test_enc_vel_dif)
test_rvel_ransac = test_enc_vel_dif * -self.wheel_radius / self.wheel_sep_ransac
plt.plot(test_enc_vel_dif, test_rvel_simple, label='simple')
plt.plot(test_enc_vel_dif, test_rvel_ransac, label='ransac')
jpu.decorate(plt.gca(),
title='wheel separation fit',
xlab='enc_diff (rad/s)',
ylab="rvel (rad/s)",
legend=True,
xlim=[-5., 5.],
ylim=[-2.5, 2.5])
jpu.savefig(filename)
def plot_training(ann):
_h = ann.history.history
plt.figure()
plt.plot(_h['loss'])
plt.plot(_h['val_loss'])
jpu.decorate(plt.gca(),
'loss',
xlab='epochs',
legend=['training', 'validation'])
def plot_training_dataset(_ann_in, _ann_out):
fig = jpu.prepare_fig(window_title='Training dataset')
names = '$y_k$', '$y_{k-1}$', '$u_{k-1}$', '$u_k$', '$y_{k+1}$'
for i in range(4):
ax = plt.subplot(1, 5, i + 1)
plt.hist(_ann_in[:, i])
jpu.decorate(ax, title=names[i])
ax = plt.subplot(1, 5, 5)
plt.hist(_ann_out)
jpu.decorate(ax, title=names[4])
def plot_interp(_d, _stamps, _d_1, _stamps_1):
plt.plot(_stamps, _d, '.', label='orig')
plt.plot(_stamps_1, _d_1, '.', label='interp')
jpu.decorate(plt.gca(),
title='Interp',
xlab='time in s',
ylab='',
legend=True,
xlim=None,
ylim=None)
def plot_test(ax1_, ax2_, stamp, X, U, Xpred):
plt.sca(ax1_)
plt.plot(stamp, X, label='truth')
plt.plot(stamp, Xpred, label='predicted')
jpu.decorate(ax1_,
title='Linear velocity',
xlab='time in s',
ylab='m/s',
legend=True)
plt.sca(ax2_)
#plt.plot(stamp, U, label='sum pwm')
plt.fill_between(stamp, U, label='sum pwm')
jpu.decorate(ax2_, title='U', xlab='time in s', ylab='', legend=True)
def plot2d(_ds, filename=None):
fig = jpu.prepare_fig(window_title='2D')
plt.plot(_ds.truth_pos[:, 0], _ds.truth_pos[:, 1], '.', label='truth')
jpu.decorate(plt.gca(),
title='truth 2D points',
xlab='m',
ylab='m',
legend=True,
xlim=None,
ylim=None)
plt.axis('equal')
jpu.savefig(filename)
return fig
def simulate_plant_and_ann(plant, ann_plant):
time = np.arange(0., 25.05, plant.dt)
sp, yp, ya = scipy.signal.square(0.25 * time), np.zeros(
len(time)), np.zeros(len(time))
for k in range(1, len(time) - 1):
yp[k + 1] = plant.io_dyn(yp[k], yp[k - 1], sp[k], sp[k - 1])
ya[k + 1] = ann_plant.predict(ya[k], ya[k - 1], sp[k - 1], sp[k])
fig = jpu.prepare_fig(window_title='Time simulation')
ax = plt.gca()
plt.plot(time, sp, label='sp')
plt.plot(time, yp, label='plant')
plt.plot(time, ya, label='ann')
jpu.decorate(ax, title='$y$', xlab='time in s', ylab='m', legend=True)
def ident(_ds):
ax = plt.subplot(1, 2, 1)
#plt.plot(_ds.data['lw_pwm'], _ds.data['lw_rvel_f'], '.')
plt.scatter(_ds.data['lw_pwm'], _ds.data['lw_rvel_f'], s=0.1)
jpu.decorate(ax, title="left wheel", xlab='lw_pwm',
ylab='lw_rvel') #, legend=True)
ax = plt.subplot(1, 2, 2)
#plt.plot(_ds.data['rw_pwm'], _ds.data['rw_rvel_f'], '.')
plt.scatter(_ds.data['rw_pwm'], _ds.data['rw_rvel_f'], s=0.1)
jpu.decorate(ax, title="right wheel", xlab='rw_pwm',
ylab='rw_rvel') #, legend=True)
#ident_poly(_ds, order=1)
#ident_spline(_ds)
ident_ann(_ds)
def plot_ctl(time, ysp, yr, y):
plt.figure()
ax = plt.subplot(2, 1, 1)
plt.plot(time, ysp, label='sp')
plt.plot(time, yr, label='ref')
plt.plot(time, y, label='plant')
jpu.decorate(ax, title='$y$', xlab='time in s', ylab='m', legend=True)
ax = plt.subplot(2, 1, 2)
plt.plot(time, y - yr, label='track err')
jpu.decorate(ax,
title='$\epsilon$',
xlab='time in s',
ylab='m',
legend=True)
def plot_imu(_ds, filename=None):
figsize = (20.48, 5.12)
fig = jpu.prepare_fig(window_title='IMU', figsize=figsize)
ax = plt.subplot(2, 1, 1)
plt.plot(_ds.enc_stamp, np.rad2deg(_ds.pitch), '.', label="pitch")
jpu.decorate(ax,
title='imu pitch',
xlab='time in s',
ylab='deg',
legend=True)
ax = plt.subplot(2, 1, 2)
plt.plot(_ds.enc_stamp, np.rad2deg(_ds.pitch_dot), '.', label="pitch_dot")
jpu.decorate(ax,
title='imu pitch dot',
xlab='time in s',
ylab='deg/s',
legend=True)
def plot_kinematics_truth(_ds, filename=None):
#fig = jpu.prepare_fig(window_title='Encoders 3D')
fig = hciods.plot_encoders_3D(_ds)
ax = fig.add_subplot(121, projection='3d')
X = np.arange(-1, 1, 0.01)
Y = np.arange(-1, 1, 0.01)
X, Y = np.meshgrid(X, Y)
lvel, rvel = np.zeros_like(X), np.zeros_like(X)
for i in range(X.shape[0]):
for j in range(X.shape[1]):
lvel[i, j], rvel[i, j] = hcu.kin_vel_of_wheel_vel(
X[i, j], Y[i, j], _ds.w_r, _ds.w_s)
#pdb.set_trace()
ax = fig.add_subplot(121, projection='3d')
surf = ax.plot_surface(X,
Y,
lvel,
cmap=matplotlib.cm.coolwarm,
linewidth=0,
antialiased=True,
alpha=0.5)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, _ds.truth_lvel,
s=0.1) #, c=c, marker=m)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$V (m/s)$')
jpu.decorate(ax, title='Linear Velocity vs/enc vels')
ax = fig.add_subplot(122, projection='3d')
surf = ax.plot_surface(X,
Y,
rvel,
cmap=matplotlib.cm.coolwarm,
linewidth=0,
antialiased=True,
alpha=0.5)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, _ds.truth_rvel,
s=0.1) #, c=c, marker=m)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$\Omega (rad/s)$')
jpu.decorate(ax, title='Angular Velocity vs/enc vels')
jpu.savefig(filename)
def plot_encoders_3D(_ds, filename=None, title=''):
fig = jpu.prepare_fig(window_title='Encoders 3D ({})'.format(title))
ax = fig.add_subplot(121, projection='3d')
truth_lvel_body_1 = interpolate(_ds.truth_lvel_body, _ds.truth_vel_stamp,
_ds.enc_vel_stamp)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, truth_lvel_body_1[:, 0], s=0.1)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$V (m/s)$')
jpu.decorate(ax, title='Linear Velocity vs/enc vels')
ax = fig.add_subplot(122, projection='3d')
truth_rvel_1 = interpolate(_ds.truth_rvel, _ds.truth_vel_stamp,
_ds.enc_vel_stamp)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, truth_rvel_1[:, 2],
s=0.1) #, c=c, marker=m)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$\Omega (rad/s)$')
jpu.decorate(ax, title='Angular Velocity vs/enc vels')
jpu.savefig(filename)
return fig
def plot_foo(ds, X, U):
fig = jpu.prepare_fig(window_title='Foo')
ax = plt.subplot(3, 1, 1)
plt.plot(ds.truth_vel_stamp, ds.truth_lvel_body[:, 0], '.', label='vx')
plt.plot(ds.truth_vel_stamp, X[:, 0], label='vx')
jpu.decorate(ax,
title='linear velocity',
xlab='time in s',
ylab='m/s',
legend=True)
ax = plt.subplot(3, 1, 2)
plt.plot(ds.truth_vel_stamp, ds.truth_rvel[:, 2], '.', label='r')
plt.plot(ds.truth_vel_stamp, X[:, 1], label='r')
jpu.decorate(ax,
title='angular velocity',
xlab='time in s',
ylab='rad/s',
legend=True)
ax = plt.subplot(3, 1, 3)
plt.plot(_ds.enc_stamp, _ds.lw_pwm, '.', label='lw_pwm')
plt.plot(_ds.enc_stamp, _ds.rw_pwm, '.', label='rw_pwm')
plt.plot(_ds.enc_stamp, U[:, 0])
plt.plot(_ds.enc_stamp, U[:, 1])
jpu.decorate(ax, title='pwm', xlab='time in s', ylab='', legend=True)
def plot(self):
ax = plt.subplot(2, 2, 1)
plt.plot(self.data['stamp'], self.data['lw_angle'], label='lw_angle')
plt.plot(self.data['stamp'], self.data['rw_angle'], label='rw_angle')
jpu.decorate(ax, title="angles", xlab='time', ylab='rad', legend=True)
ax = plt.subplot(2, 2, 2)
plt.plot(self.data['stamp'],
self.data['lw_rvel'],
alpha=0.5,
label='lw_rvel')
plt.plot(self.data['stamp'],
self.data['rw_rvel'],
alpha=0.5,
label='rw_rvel')
plt.plot(self.data['stamp'], self.data['lw_rvel_f'], label='lw_rvel_f')
plt.plot(self.data['stamp'], self.data['rw_rvel_f'], label='rw_rvel_f')
jpu.decorate(ax, title="rvel", xlab='time', ylab='rad/s', legend=True)
ax = plt.subplot(2, 2, 3)
plt.plot(self.data['stamp'], self.data['lw_pwm'], label='lw_pwm')
plt.plot(self.data['stamp'], self.data['rw_pwm'], label='rw_pwm')
jpu.decorate(ax,
title="pwm",
xlab='time',
ylab='-127,127',
legend=True)
def plot_err(odom_pos, odom_yaw, ds, label, start_idx, run_len, filename):
figsize = (20.48, 10.24)
margins = 0.05, 0.07, 0.97, 0.95, 0.14, 0.39
fig = jpu.prepare_fig(window_title='Odometry error {} ({})'.format(
label, filename),
figsize=figsize,
margins=margins)
if run_len is None:
run_len = len(ds.enc_stamp) - start_idx # we go to the end
truth_pos_1 = hio.interpolate(ds.truth_pos, ds.truth_stamp,
ds.enc_stamp[start_idx:start_idx + run_len])
pos_err = odom_pos - truth_pos_1
ax = plt.subplot(2, 1, 1)
plt.plot(ds.enc_stamp[start_idx:start_idx + run_len],
np.linalg.norm(pos_err, axis=1))
jpu.decorate(ax,
title='odom translation error',
xlab='time in s',
ylab='m',
legend=None,
xlim=None,
ylim=None)
truth_yaw = np.array([
tf.transformations.euler_from_quaternion(q, axes='sxyz')[2]
for q in ds.truth_ori
])
truth_yaw_1 = hio.interpolate(truth_yaw[:, np.newaxis], ds.truth_stamp,
ds.enc_stamp[start_idx:start_idx + run_len])
yaw_err = hcu.wrap_angle(odom_yaw - truth_yaw_1)
ax = plt.subplot(2, 1, 2)
plt.plot(ds.enc_stamp[start_idx:start_idx + run_len], np.rad2deg(yaw_err))
jpu.decorate(ax,
title='odom rotation error',
xlab='time in s',
ylab='deg',
legend=None,
xlim=None,
ylim=None)
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 plot_encoders_stats(_ds, filename=None, title=''):
''' plot encoders histograms '''
fig = jpu.prepare_fig(window_title='Encoders Stats ({})'.format(title))
ax = plt.subplot(2, 2, 1)
plt.hist(_ds.enc_vel_lw)
jpu.decorate(ax, title='Left Wheel', xlab='rvel in rad/s', ylab='samples')
ax = plt.subplot(2, 2, 2)
plt.hist(_ds.enc_vel_lw)
jpu.decorate(ax, title='Right Wheel', xlab='rvel in rad/s', ylab='samples')
ax = plt.subplot(2, 2, 3)
plt.hist(_ds.truth_lvel_body[:, 0])
jpu.decorate(ax, title='Linear Velocity', xlab='m/s', ylab='samples')
ax = plt.subplot(2, 2, 4)
plt.hist(_ds.truth_rvel[:, 2])
jpu.decorate(ax, title='Angular Velocity', xlab='rad/s', ylab='samples')
jpu.savefig(filename)
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 plot_encoders(_ds, filename=None):
fig = jpu.prepare_fig(window_title='Encoders')
ax = plt.subplot(4, 1, 1)
plt.plot(_ds.enc_stamp, _ds.enc_lw, '.', label="lw")
plt.plot(_ds.enc_stamp, _ds.enc_rw, '.', label="rw")
jpu.decorate(ax,
title='Encoders position',
xlab='time in s',
ylab='rad',
legend=True)
ax = plt.subplot(4, 1, 2)
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_lw, '.')
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_rw, '.')
jpu.decorate(ax,
title='Encoders velocity',
xlab='time in s',
ylab='rad/s',
legend=None,
xlim=None,
ylim=None)
ax = plt.subplot(4, 1, 3)
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_sum, '.')
jpu.decorate(ax,
title='Encoders velocity sum',
xlab='time in s',
ylab='rad/s',
legend=None,
xlim=None,
ylim=None)
ax = plt.subplot(4, 1, 4)
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_dif, '.')
jpu.decorate(ax,
title='Encoders velocity dif',
xlab='time in s',
ylab='rad/s',
legend=None,
xlim=None,
ylim=None)
jpu.savefig(filename)
def sim_cl_ann(ann, K, dt=0.01):
time = np.arange(0, 15, dt)
X, U = np.zeros((len(time), 6)), np.zeros((len(time), 2))
phi0, gamma0, theta0 = np.deg2rad(1), np.deg2rad(1), np.deg2rad(1)
X[0] = [phi0, gamma0, 0, 0, theta0, 0]
for k in range(1, len(time)):
U[k - 1] = -np.dot(K, X[k - 1])
_in_km1 = np.hstack((X[k - 1], U[k - 1]))[np.newaxis, :]
X[k] = ann.predict(_in_km1)
ax = plt.subplot(3, 1, 1)
plt.plot(time, np.rad2deg(X[:, 0]))
jpu.decorate(ax, title='phi', xlab='time in s', ylab='deg', legend=True)
ax = plt.subplot(3, 1, 2)
plt.plot(time, np.rad2deg(X[:, 1]))
jpu.decorate(ax, title='gamma', xlab='time in s', ylab='deg', legend=True)
ax = plt.subplot(3, 1, 3)
plt.plot(time, np.rad2deg(X[:, 4]))
jpu.decorate(ax, title='theta', xlab='time in s', ylab='deg', legend=True)
plt.show()
def plot_training(ann):
fig = jpu.prepare_fig(window_title='Training history')
_h = ann.history.history
plt.plot(_h['loss'], label='loss')
plt.plot(_h['val_loss'], label='val_loss')
jpu.decorate(plt.gca(), 'loss', xlab='epochs', 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))
def plot_residuals(self, info, filename=None):
figsize = (20.48, 10.24)
margins = 0.05, 0.07, 0.97, 0.95, 0.14, 0.39
fig = jpu.prepare_fig(
window_title='Odometry parameters regression ({})'.format(info),
figsize=figsize,
margins=margins)
inlier_mask = self.ransac_wr.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
ax = plt.subplot(4, 1, 1)
#plt.scatter(self.ds.enc_vel_stamp, self.res_radius_simple, marker='.')
plt.scatter(self.ds.enc_vel_stamp[outlier_mask],
self.res_radius_ransac[outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_stamp[inlier_mask],
self.res_radius_ransac[inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
jpu.decorate(ax,
title='wheel radius residuals',
xlab='time in s',
ylab='m',
legend=True,
xlim=None,
ylim=None)
_fmt_legend = '$\mu: {:.2e} \quad \sigma: {:.2e}$'
ax = plt.subplot(4, 2, 3)
plt.hist(self.res_radius_simple, normed=True, bins=30)
jpu.decorate(ax,
title='simple',
xlab='m',
legend=[
_fmt_legend.format(self.res_radius_simple_mu,
self.res_radius_simple_sigma)
])
ax = plt.subplot(4, 2, 4)
plt.hist(self.res_radius_ransac[self.ransac_wr.inlier_mask_],
normed=True,
bins=30)
jpu.decorate(ax,
title='ransac',
xlab='m',
legend=[
_fmt_legend.format(self.res_radius_ransac_mu,
self.res_radius_ransac_sigma)
])
inlier_mask = self.ransac_ws.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
ax = plt.subplot(4, 1, 3)
#plt.scatter(self.ds.enc_vel_stamp, self.res_wheel_sep_simple, marker='.')
plt.scatter(self.ds.enc_vel_stamp[outlier_mask],
self.res_wheel_sep_ransac[outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_stamp[inlier_mask],
self.res_wheel_sep_ransac[inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
jpu.decorate(ax,
title='wheel separation residuals',
xlab='time in s',
ylab='',
legend=True,
xlim=None,
ylim=None)
ax = plt.subplot(4, 2, 7)
plt.hist(self.res_wheel_sep_simple, normed=True, bins=30)
jpu.decorate(ax,
title='simple',
legend=[
_fmt_legend.format(self.res_wheel_sep_simple_mu,
self.res_wheel_sep_simple_sigma)
])
ax = plt.subplot(4, 2, 8)
plt.hist(self.res_wheel_sep_ransac[self.ransac_ws.inlier_mask_],
normed=True,
bins=30)
jpu.decorate(ax,
title='ransac',
legend=[
_fmt_legend.format(self.res_wheel_sep_ransac_mu,
self.res_wheel_sep_ransac_sigma)
])
jpu.savefig(filename)
def plot_time_distribution(self):
self.enc_dt = self.data['stamp'][1:] - self.data['stamp'][:-1]
plt.hist(self.enc_dt, bins=100)
jpu.decorate(plt.gca(), title="timestamps", xlab='s', ylab='')
def plot_training_history(self, filename=None):
fig = jpu.prepare_fig(window_title='Training History')
ax = plt.subplot(1,1,1)
plt.plot(self.history.history['loss'], label='loss')
plt.plot(self.history.history['val_loss'], label='val_loss')
jpu.decorate(ax, title='loss', legend=True);
