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)