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_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 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_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_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 make_many_test(ann, _dir, _fns):
_t = 'homere'
fig = jpu.prepare_fig(window_title='Test Plant Prediction')
nr, nc = 4, 2
for i, _fn in enumerate(_fns):
ds = hio.DataSet(_dir + _fn, _t)
X, U, Xpred = test_plant(ann, ds)
lvu.plot_test(plt.subplot(nr, nc, nc * i + 2),
plt.subplot(nr, nc, nc * i + 1), ds.enc_vel_stamp, X, U,
Xpred)
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 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 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 plot_io_chronogram(self, _input, _output, _time):
figsize=(20.48, 5.12)
fig = jpu.prepare_fig(window_title='Input/Output', figsize=figsize)
plots = [(np.rad2deg(_input[:,0]), 'phi', 'deg'),
(np.rad2deg(_input[:,1]), 'gamma', 'deg'),
(np.rad2deg(_input[:,4]), 'theta', 'deg'),
(np.rad2deg(_input[:,2]), 'phi dot', 'deg/s'),
(np.rad2deg(_input[:,3]), 'gamma dot', 'deg/s'),
(np.rad2deg(_input[:,5]), 'theta dot', 'deg'),
(_input[:,6], 'pwm l', ''),
(_input[:,7], 'pwm r', '')
]
for i, (_d, _n, _u) in enumerate(plots):
ax = plt.subplot(3,3,i+1)
#pdb.set_trace()
plt.plot(_time, _d)
jpu. decorate(ax, title=_n, xlab='time in s', ylab=_u)
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_dataset(_ds):
#iod.plot_imu(_ds)
integ_pitch = np.zeros(len(_ds.enc_stamp))
integ_pitch[0] = _ds.pitch[0]
for k in range(1, len(_ds.enc_stamp)):
integ_pitch[k] = integ_pitch[k-1] + _ds.pitch_dot[k-1]*(_ds.enc_stamp[k]-_ds.enc_stamp[k-1])
#pdb.set_trace()
truth_pitch = np.array([hcu.rpy_of_q(_q) for _q in _ds.truth_ori])
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="IMU")
#plt.plot(_ds.truth_stamp, np.rad2deg(truth_pitch[:,1]), label='truth')
plt.plot(_ds.enc_stamp, np.rad2deg(integ_pitch), label='integrated IMU pitch dot')
jpu. decorate(ax, title='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="IMU")
#plt.plot(_ds.truth_vel_stamp, np.rad2deg(_ds.truth_rvel[:,1]), label='truth')
jpu. decorate(ax, title='Pitch dot', xlab='time in s', ylab='deg/s', legend=True)
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_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 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 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);
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_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)