def __init__(self, _nb_points, dt=0.01, enc_mu=0., enc_std=0.1):
self.w_r, self.w_s = 0.2, 0.5
if 0:
self.enc_vel_lw, self.enc_vel_rw = [
np.random.uniform(low=-1., high=1., size=_nb_points)
for i in range(2)
]
else:
length, angle = np.sqrt(np.random.uniform(
0, 1, size=_nb_points)), np.pi * np.random.uniform(
0, 2, size=_nb_points)
self.enc_vel_lw, self.enc_vel_rw = length * np.cos(
angle), length * np.sin(angle)
self.enc_vel_stamp = np.arange(0, 0 + _nb_points * dt, dt)
err_lw, err_rw = [
np.random.normal(loc=enc_mu, scale=enc_std, size=_nb_points)
for i in range(2)
]
foo = np.array([
hcu.kin_vel_of_wheel_vel(lrv, rrv, self.w_r, self.w_s)
for lrv, rrv in zip(self.enc_vel_lw + err_lw, self.enc_vel_rw +
err_rw)
])
self.truth_lvel, self.truth_lvel_body, self.truth_rvel = [
np.zeros((_nb_points, 3)) for i in range(3)
]
self.truth_lvel[:, 0] = self.truth_lvel_body[:, 0] = foo[:, 0]
self.truth_rvel[:, 2] = foo[:, 1]
self.truth_vel_stamp = self.enc_vel_stamp
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)
def test_plant(plant_ann, ds):
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.enc_vel_lw, ds.enc_vel_rw)
])
pwm_sum = ds.lw_pwm + ds.rw_pwm
U = pwm_sum[:-1]
Xpred = np.zeros((len(ds.enc_vel_stamp), 1))
Xpred[0] = lr_vels_from_enc[0, 0]
Xpred[1] = lr_vels_from_enc[1, 0]
for i in range(2, len(ds.enc_vel_stamp)):
Xpred[i] = plant_ann.predict(Xpred[i - 1], Xpred[i - 2], U[i - 1],
U[i - 2])
X = lr_vels_from_enc[:, 0]
return X, U, Xpred
def test(ann, 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_test.enc_vel_lw, ds_test.enc_vel_rw)
])
pwm_sum = _ds_test.lw_pwm + _ds_test.rw_pwm
U = pwm_sum[:-1]
Xpred = np.zeros((len(ds_test.enc_vel_stamp), 1))
Xpred[0] = lr_vels_from_enc[0, 0]
Xpred[1] = lr_vels_from_enc[1, 0]
for i in range(2, len(ds_test.enc_vel_stamp)):
Xpred[i] = ann.predict(Xpred[i - 1], Xpred[i - 2], U[i - 1], U[i - 2])
hio.plot_truth_vel(ds_test)
plt.subplot(2, 1, 1)
plt.plot(ds_test.enc_vel_stamp, Xpred[:, 0])
def prepare_dataset(self, ds):
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.enc_vel_lw, ds.enc_vel_rw)
])
pwm_sum = ds.lw_pwm + ds.rw_pwm
X = lr_vels_from_enc[:, 0]
U = pwm_sum[:-1]
_ann_out = X[self.delay:]
_ann_in = np.zeros((len(X) - self.delay, self.input_size))
for i in range(self.delay, len(X)):
_ann_in[i - self.delay, self.v_km1] = X[i - 1]
_ann_in[i - self.delay, self.v_km2] = X[i - 2]
_ann_in[i - self.delay, self.u_km1] = U[i - 1]
_ann_in[i - self.delay, self.u_km2] = U[i - 2]
return _ann_in, _ann_out
def test_plant(plant_ann, ds):
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.enc_vel_lw, ds.enc_vel_rw)
])
pwm_sum, pwm_dif = ds.lw_pwm + ds.rw_pwm, ds.lw_pwm - ds.rw_pwm
U = np.vstack((pwm_sum, pwm_dif)).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] = plant_ann.predict(Xpred[i - 1, 0], Xpred[i - 1, 1],
Xpred[i - 2, 0], Xpred[i - 2, 1], U[i - 1,
0],
U[i - 1, 1], U[i - 2, 0], U[i - 2, 1])
X = lr_vels_from_enc
return X, U, Xpred
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 test(ann, ds):
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.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])
def run(ds, ds2):
wr, ws = 0.2, 0.75
#wr, ws = 0.2350, 1.1248
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.enc_vel_lw, ds.enc_vel_rw)
])
if 1:
hio.plot_truth_vel(_ds)
plt.subplot(2, 1, 1)
plt.plot(ds.enc_vel_stamp, lr_vels_from_enc[:, 0])
plt.subplot(2, 1, 2)
plt.plot(ds.enc_vel_stamp, lr_vels_from_enc[:, 1])
ann = SklearnAnn()
#X = np.vstack((ds.truth_lvel_body[:,0], ds.truth_rvel[:,2])).T
X = lr_vels_from_enc
U = np.vstack((_ds.lw_pwm, _ds.rw_pwm)).T[:-1]
#plot_foo(ds, X, U)
ann.fit(X, U)
ann.summary()
test(ann, ds2)
def prepare_dataset(self, ds):
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.enc_vel_lw, ds.enc_vel_rw)
])
pwm_sum, pwm_dif = ds.lw_pwm + ds.rw_pwm, ds.lw_pwm - ds.rw_pwm
X = lr_vels_from_enc
U = np.vstack((pwm_sum, pwm_dif)).T[:-1]
_ann_out = X[self.delay:]
_ann_in = np.zeros((len(X) - self.delay, self.input_size))
for i in range(self.delay, len(X)):
_ann_in[i - self.delay, self.lv_km1] = X[i - 1, 0]
_ann_in[i - self.delay, self.rv_km1] = X[i - 1, 1]
_ann_in[i - self.delay, self.lv_km2] = X[i - 2, 0]
_ann_in[i - self.delay, self.rv_km2] = X[i - 2, 1]
_ann_in[i - self.delay, self.ps_km1] = U[i - 1, 0]
_ann_in[i - self.delay, self.pd_km1] = U[i - 1, 1]
_ann_in[i - self.delay, self.ps_km2] = U[i - 2, 0]
_ann_in[i - self.delay, self.pd_km2] = U[i - 2, 1]
return _ann_in, _ann_out