# testing trajectories
test_trajectory_left = read_csv_fast('trajectories/test_left_100.csv')
test_trajectory_right = read_csv_fast('trajectories/test_right_100.csv')
test_trajectory_straight = read_csv_fast('trajectories/test_straight_100.csv')
# creation of basis function
N = 100
######## 1st Radial Basis Function ########
x1 = np.random.normal(0, 0.1, (N, 2))
y1 = np.exp(-(x1 - 0)**2 / 0.2**2)
print(x1)
# noise = 0.00001
interpolated_right = interpolate_dist(test_trajectory_right[:, 1],
test_trajectory_right[:, 2], N)
x_test_tr_right = interpolated_right[
0] # x coordinates of test trajectory for going to the right
y_test_tr_right = interpolated_right[
1] # x coordinates of test trajectory for going to the right
# weights calculation -> least mean squares
f = pa.filters.FilterLMS(n=2, mu=1, w="zeros")
x_right, e_x_right, w_x_right = f.run(x_test_tr_right, x1)
print("weights_right_x: ", len(w_x_right), w_x_right)
y_right, e_y_right, w_y_right = f.run(y_test_tr_right, x1)
print("weights_right_y: ", len(w_y_right), w_y_right)
# # plot results
plt.figure(figsize=(8, 5))
Number_Of_Points = 100
# fill files directionary with file paths of all csv files
files_dictionary = {
'left': glob.glob('trajectories/left_*.csv'
), # return all files starting with left_ in the folder
'right': glob.glob('trajectories/right_*.csv'
), # return all files starting with right in the folder
'straight':
glob.glob('trajectories/straight_*.csv'
), # return all files starting with straight in the folder
}
random_trajectory = read_csv_fast('trajectories/test_right_100.csv')
interpolated_random_trajectory = interpolate_dist(random_trajectory[:, 1],
random_trajectory[:, 2],
Number_Of_Points)
random_trajectory = np.asarray(random_trajectory)
[xn, yn] = interpolate_dist(random_trajectory[:, 1], random_trajectory[:, 2],
Number_Of_Points)
random_trajectory = np.vstack((xn, yn)).T
"""
Step 3: Accumulate and plot all trajectories
"""
trajectory_csv_data = {} # container dictionary for ploting graphs
trajectory_csv_file_wise_data = {} # container dictionary for averaging
all_points_from_files = [
] # big array container for all points from all trajectories
# color map for graph
from functions import read_csv_fast, interpolate_dist
from scipy.interpolate import interp1d
import numpy as np
import matplotlib.pyplot as plt
random_trajectory_right = read_csv_fast('trajectories/right_03.csv')
interpolated_trajectory_right = interpolate_dist(random_trajectory_right[:, 1],
random_trajectory_right[:, 2],
10)
random_trajectory_straight = read_csv_fast('trajectories/straight_03.csv')
interpolated_trajectory_straight = interpolate_dist(
random_trajectory_straight[:, 1], random_trajectory_straight[:, 2], 10)
random_trajectory_left = read_csv_fast('trajectories/left_03.csv')
interpolated_trajectory_left = interpolate_dist(random_trajectory_left[:, 1],
random_trajectory_left[:,
2], 10)
# labels
plt.title(
'Trajectries interpolation from original numbet of time steps to 10 time steps'
)
plt.xlabel('x')
plt.ylabel('y')
labels = {
'original': 'Original trajectory',
'interpolated': 'Original trajectory interpolated to 10 points'
}
#ax[1].plot(traj_y, 'b')
#ax[1].plot(reconstr_trajy, 'r')
#plt.show()
err = np.sum((reconstr_trajx - traj_x)**2)
print err
x_weights = np.mean(weights[:, 0], axis=0)
y_weights = np.mean(weights[:, 1], axis=0)
print("Weights for X: ", x_weights)
print("Weights for Y: ", y_weights)
###### Plotting Results ######
testing_trajectory = read_csv_fast('trajectories/test_right_100.csv')
[xn, yn] = interpolate_dist(testing_trajectory[:, 1], testing_trajectory[:, 2],
time_steps)
points = np.vstack((xn, yn)).T
# plotting basis functions
fig1, axes = plt.subplots(3, 1)
axes[0].plot(psi.T)
axes[0].set_ylim([0, 1])
# plotting x value of test trajectories
for i in range(len(dataset[0])):
axes[1].plot(dataset[i][:, 0], color="blue", alpha=0.4)
axes[2].plot(dataset[i][:, 1], color="blue", alpha=0.4)
wx = weights[i, 0]
wy = weights[i, 1]
#traj_x = dataset[i][:, 0]