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()