def data_converter(filename, type): """ Converts Drouin's data test and python classes properties into a datset compliant with my functions. filename : '/home/poine/work/homere/homere_control/data/homere_io_10.npz' type : 'homere' Out : ds = [wr, wl, V, omega], time (list of instant) len(wr) = len(time) """ original_ds = iodata.DataSet(filename, type) wl = original_ds.enc_vel_lw wr = original_ds.enc_vel_rw original_ds.truth_lvel_body = iodata.interpolate( original_ds.truth_lvel_body, original_ds.truth_vel_stamp, original_ds.enc_vel_stamp) original_ds.truth_rvel = iodata.interpolate(original_ds.truth_rvel, original_ds.truth_vel_stamp, original_ds.enc_vel_stamp) truth_vx, truth_vy = original_ds.truth_lvel_body[:, 0], original_ds.truth_lvel_body[:, 1] truth_V = [ sqrt(truth_vx[i]**2 + truth_vy[i]**2) for i in range(len(truth_vy)) ] truth_omega = original_ds.truth_rvel[:, 2] time_encoders = original_ds.enc_vel_stamp converted_ds = [wr, wl, truth_V, truth_omega] return converted_ds, time_encoders
def gen_summary_plots(examples): for _section, _section_examples in examples: for filename, _type , _name in _section_examples: _ds = odm_ds.DataSet(filename, _type) for _p_name, _f in all_summarry_plots: _f(_ds, filename=plot_dir+_name+'_{}.png'.format(_p_name)) plt.close('all')
def plot_true_orientation(filename, type): original_ds = iodata.DataSet(filename, type) plt.plot(original_ds.truth_stamp, original_ds.truth_ori, label='truth') plt.xlabel('time (sec)') plt.ylabel('thetat (rad)') #filename, type = './data/oscar_io_oval.npz', 'oscar' #ds, time = data_converter(filename, type)
def plot_true_position(filename, type): original_ds = iodata.DataSet(filename, type) plt.plot(original_ds.truth_pos[:, 0], original_ds.truth_pos[:, 1], label='truth', linestyle='--') plt.axis('equal') plt.xlabel('x (m)') plt.ylabel('y (m)')
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 main(train_plant=True): plant_ann = LRvelIoAnn() plant_ann_filename = '/home/poine/work/homere/homere_control/data/lrvel_io_plant_ann2.h5' if train_plant or not os.path.isfile(plant_ann_filename): #_fn, _t = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_11_random_lrvel.npz', 'homere' _fn, _t = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_10.npz', 'homere' _ds_train = hio.DataSet(_fn, _t) hio.plot_truth_vel(_ds_train) plant_ann.train(_ds_train, epochs=600, plant_ann_filename=plant_ann_filename) hcu.plot_training(plant_ann) plt.show() else: plant_ann.load(plant_ann_filename) if 0: _fn, _t = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_15_sine_rvel_1.npz', 'homere' _ds_test = hio.DataSet(_fn, _t) X, U, Xpred = test_plant(plant_ann, _ds_test) lvu.plot_lrvel_test(plt.subplot(1, 3, 2), plt.subplot(1, 3, 3), plt.subplot(1, 3, 1), _ds_test.enc_vel_stamp, X, U, Xpred) else: if 1: make_many_test( plant_ann, '/home/poine/work/homere/homere_control/data/homere/gazebo/', [ 'homere_io_16_step_pwm_10_sum.npz', 'homere_io_16_step_pwm_20_sum.npz', 'homere_io_16_step_pwm_30_sum.npz', 'homere_io_16_step_pwm_40_sum.npz' ]) if 1: make_many_test( plant_ann, '/home/poine/work/homere/homere_control/data/homere/gazebo/', [ 'homere_io_17_sine_pwm_sum.npz', 'homere_io_17_sine_pwm_15_sum.npz', 'homere_io_17_sine_pwm_30_sum.npz', 'homere_io_17_sine_pwm_40_sum.npz' ]) plt.show()
def data_initial_position(filename, type): original_ds = iodata.DataSet(filename, type) truth_yaw = np.array([ tf.transformations.euler_from_quaternion(q, axes='sxyz')[2] for q in original_ds.truth_ori ]) x0, y0, theta0 = original_ds.truth_pos[0][0], original_ds.truth_pos[0][ 1], truth_yaw[0] return x0, y0, theta0
def make_regression_dif_drive(examples, make_plots=False): html_filename = md_dir + '_includes/regression1.html' content = '' for _section, _section_examples in examples: content += ''' ## {} <table style="border-collapse: collapse; border-spacing: 20px; text-align: center; border: 0px solid black;"> <tr><th>Exp</th><th>Reg Radius</th><th>Reg Separation</th><th>Residuals</th></tr>\n\n'''.format(_section) for filename, _type , _name in _section_examples: _ds = odm_ds.DataSet(filename, _type) reg = odm_ft.Regression(_ds) reg.fit_wheel_radius() reg.fit_wheel_sep() if make_plots: reg.plot_residuals(info=_name, filename=plot_dir+_name+'_reg_dd1_res.png') reg.plot_wheel_radius(filename=plot_dir+_name+'_reg_dd1_wr.png') reg.plot_wheel_sep(filename=plot_dir+_name+'_reg_dd1_ws.png') plt.close('all') comment = 'size: {} enc'.format(len(_ds.enc_stamp)) _ds_2d_img = '../plots/{}_truth_2d.png'.format(_name) content += '''<tr> <td style="text-align: left; padding: 15px;" valign="top" width="50"> <div class="hover_img"> <a href="../examples#{}">{} <span><img src="{}" alt="image" height="480" /></span> </a> </div> {} </td> '''.format(_name, _name, _ds_2d_img, comment) for _p_name, _fp, _fr in all_reg1_plots: img_with_link = write_img_link('../plots/{}_{}.png'.format(_name, _p_name)) desc = _fr(reg) if _fr is not None else '' content += ' <td valign="top">\n {}\n <br>\n\n<div markdown="1" style="float: right">\n\n{}\n\n</div>\n\n </td>\n'.format(img_with_link, desc) content+= "</tr>\n" content += '\n</table>\n' write_html_include(html_filename, content)
jpu.decorate(ax, title='odom rotation error', xlab='time in s', ylab='deg', legend=None, xlim=None, ylim=None) if __name__ == '__main__': #ds = hio.DataSet('/home/poine/work/homere/homere_control/data/odom_gazebo_2.npz', _type='homere') #ds = hio.DataSet('/home/poine/work/oscar.git/oscar/oscar_control/scripts/odometry/odom_data_4.npz', _type='oscar') filename, _type = '/home/poine/work/julie/julie/julie_control/scripts/julie_odom_data_1.npz', 'homere' #filename, _type = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_3.npz', 'homere' ds = hio.DataSet(filename, _type) reg = fod.Regression(ds) reg.fit_wheel_radius() reg.fit_wheel_sep() #wheel_sep, wheel_radius = 0.11039085, 0.03078012 wheel_sep, wheel_radius = reg.wheel_sep, reg.wheel_radius if True: # python implementation odom, label = Odometer(wheel_sep, wheel_radius, wheel_radius), 'pyodom' _start_idx, _run_len = 0, None odom_pos, odom_yaw = run_on_ds(odom, ds, label, _start_idx, _run_len) plot2d(odom, odom_pos, odom_yaw, ds, label) plot_err(odom_pos, odom_yaw, ds, label, _start_idx, _run_len, filename) #pdb.set_trace() if False: # C++ implementation odom2, label = cpp_homere_control.Odometry(), 'cppodom'
_unused, self.alpha = self.ctl.compute_looped(p0, psi) self.publish_ackermann_cmd(self.alpha, self.vel_sp.get_pwm(rospy.Time.now().to_sec())) except ros_utils.RobotLostException: print('robot lost\r') except ros_utils.RobotNotLocalizedException: print('robot not localized\r') def publish_ackermann_cmd(self, alpha, v): msg = ackermann_msgs.msg.AckermannDriveStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'odom' msg.drive.steering_angle = alpha msg.drive.speed = v self.pub_ack.publish(msg) def run(self): self.rate = rospy.Rate(20.) while not rospy.is_shutdown(): self.periodic() self.rate.sleep() if __name__ == '__main__': rospy.init_node('lvel_calibration') if 1: # send calibration inputs Node().run() if 0: # compute calibration from recorded data ds = iods.DataSet('/tmp/oscar_io.npz', 'oscar') iods.plot_encoders(ds) plt.show()
if __name__ == '__main__': keras.backend.set_floatx('float64') np.set_printoptions(precision=2, linewidth=300) #filename, _type = '/home/poine/work/homere/homere_control/data/rosmip/gazebo/rosmip_io_01_step_lin.npz', 'rosmip' #filename, _type = '/home/poine/work/homere/homere_control/data/rosmip/gazebo/rosmip_io_04_sine_2.npz', 'rosmip' #filename, _type = '/home/poine/work/homere/homere_control/data/rosmip/gazebo/rosmip_io_05_random_2.npz', 'rosmip' #filename, _type = '/home/poine/work/homere/homere_control/data/rosmip/gazebo/rosmip_io_06_step_misc.npz', 'rosmip' #filename, _type = '/mnt/mint18/home/poine/work/homere/homere_control/data/rosmip/gazebo/rosmip_io_07_random_2.npz', 'rosmip' filename, _type = '/mnt/mint18/home/poine/work/homere/homere_control/data/rosmip/gazebo/rosmip_2_io_08_random_2.npz', 'rosmip' if len(sys.argv) >= 2: filename = sys.argv[1] ds = iod.DataSet(filename, _type) # there's a bug in here # we use truth rather than IMU in simulation plot_dataset(ds) plt.show() ann = ANN() _input, _output, _time = ann.make_io(ds) ann_filename = '/tmp/rosmip_ann.h5' if True: ann.train(_input, _output, _time) ann.save(ann_filename) ann.plot_training_history() else: ann.load(ann_filename) ann.report()
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) if __name__ == '__main__': if 1: name, _ds = 'random', hciods.RandomDataset(int(5e3)) else: #filename, _type = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_10.npz', 'homere' #name, _ds = 'homere_gazebo_io_10', hciods.DataSet(filename, _type) #filename, _type, name = '/home/poine/work/oscar.git/oscar/oscar_control/scripts/odometry/odom_data_4.npz', 'oscar', 'oscar_smocap_4' filename, _type, name = '/home/poine/work/homere/homere_control/data/odometry/julie/gazebo_4.npz', 'julie', 'julie_gazebo_4' _ds = hciods.DataSet(filename, _type) hciods.plot2d(_ds, '/tmp/{}_enc_2D.png'.format(name)) plt.show() #hciods.plot_encoders_stats(_ds, '/tmp/{}_enc_stats.png'.format(name), name) #hciods.plot_encoders_3D(_ds, '/tmp/{}_enc_3D.png'.format(name), name) plot_kinematics_truth(_ds, '/tmp/{}_odometry_3D.png') plt.show()
for lw_rvel, rw_rvel in zip(ds.enc_vel_lw, ds.enc_vel_rw) ]) U = np.vstack((ds.lw_pwm, ds.rw_pwm)).T[:-1] Xpred = np.zeros((len(ds.enc_vel_stamp), 2)) Xpred[0] = lr_vels_from_enc[0] Xpred[1] = lr_vels_from_enc[1] for i in range(2, len(ds.enc_vel_stamp)): Xpred[i] = ann.predict(Xpred[i - 1], Xpred[i - 2], U[i - 1], U[i - 2]) #pdb.set_trace() #plt.hist() hio.plot_truth_vel(ds) plt.subplot(2, 1, 1) plt.plot(ds.enc_vel_stamp, Xpred[:, 0]) plt.subplot(2, 1, 2) plt.plot(ds.enc_vel_stamp, Xpred[:, 1]) if __name__ == '__main__': filename, _type = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_10.npz', 'homere' #filename, _type = '/home/poine/work/homere/homere_control/data/odometry/julie/gazebo_3.npz', 'homere' _ds = hio.DataSet(filename, _type) _ds2 = hio.DataSet( '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_4.npz', 'homere') #hio.plot_encoders(_ds) #hio.plot2d(_ds) #hio.plot_truth_vel(_ds) run(_ds, _ds2) plt.show()
plt.plot(ds_test.enc_vel_stamp, Xpred[:, 0]) def run(ds_train, ds_test): wr, ws = 0.2, 0.75 lr_vels_from_enc = np.array([ hcu.kin_vel_of_wheel_vel(lw_rvel, rw_rvel, wr, ws) for lw_rvel, rw_rvel in zip(ds_train.enc_vel_lw, ds_train.enc_vel_rw) ]) X = lr_vels_from_enc[:, 0] pwm_sum = _ds_train.lw_pwm + _ds_train.rw_pwm U = pwm_sum[:-1] ann = LvelIoAnn() ann.fit(X, U) ann.summary() test(ann, ds_test) #pdb.set_trace() if __name__ == '__main__': _fn, _t = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_11_random_lrvel.npz', 'homere' _ds_train = hio.DataSet(_fn, _t) #hio.plot_all(_ds_train) _fn, _t = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_16_step_pwm_sum.npz', 'homere' #_fn, _t = '/home/poine/work/homere/homere_control/data/homere/gazebo/homere_io_13_random_pwm_sum.npz', 'homere' _ds_test = hio.DataSet(_fn, _t) run(_ds_train, _ds_test) plt.show()