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