def test_network(network, x_true, y_true, vel, t):
# Initialize truck simulator and control objects
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
controller = NN2Control()
pid = StanleyPID()
x = []
th1t = 0
th2t = 0
#do some random stuff
ego2 = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
for i in range(0, len(t)):
network = network.float()
state = ego.convert_world_state_to_front()
state1 = ego2.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t, network)
ctrl_delta_pid, ctrl_vel_pid, err_pid, interr_pid, differr_pid = pid.calc_steer_control(
t[i], state1, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x1, y1, delt, tha, thb = ego2.simulate_timestep(
[ctrl_vel_pid, ctrl_delta_pid])
x.append(err[0] * err[0])
x = sum(x) / len(t)
return x
def ego_ol_test():
'''
Open-loop test of truck-trailer simulation. Takes the data from a Simpack
CSV and simulates the open-loop output using the Simpack-recorded control
signals. Plots the resulting driven path against the Simpack-recorded path.
'''
x_true, y_true, t, vel, steer_angle = read_path_from_original_simpack_csv(
'/Users/Zeke/Documents/MATLAB/Model Validation/P4_TCO_Sleeper__FE17_Trailer/DLC/Benchmark_DLC_31ms_pandas.csv'
)
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
x = []
y = []
delta = []
th1 = []
th2 = []
for i in range(0, len(t)):
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[vel[i], steer_angle[i]])
x.append(xt)
y.append(yt)
delta.append(deltat)
th1.append(th1t)
th2.append(th2t)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.show()
def random_path_pid(output_to_csv=True, csv_filename='random_path_pid.csv'):
rpg = RandomPathGenerator()
x_true, y_true, t, vel = rpg.get_random_path()
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
pid = StanleyPID()
ctrl_delta_out, ctrl_vel_out, ct_err, hd_err, ct_err_int, hd_err_int, \
ct_err_diff, hd_err_diff = [], [], [], [], [], [], [], []
for i in range(0, len(t)):
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, err_int, err_diff = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
ego.simulate_timestep([ctrl_vel, ctrl_delta])
if not np.isnan(err[0]):
ctrl_delta_out.append(ctrl_delta)
ctrl_vel_out.append(ctrl_vel)
ct_err.append(err[0])
hd_err.append(err[1])
ct_err_int.append(err_int[0])
hd_err_int.append(err_int[1])
ct_err_diff.append(err_diff[0])
hd_err_diff.append(err_diff[1])
csv_output = {
'Cross-Track Error (m)': ct_err,
'Heading Error (rad)': hd_err,
'Desired Velocity (m/s)': ctrl_vel_out,
'Command Steer Angle (rad)': ctrl_delta_out,
'Integral of CT Error': ct_err_int,
'Integral of Heading Error': hd_err_int,
'Derivative of CT Error': ct_err_diff,
'Derivative of Heading Error': hd_err_diff
}
df = pd.DataFrame(csv_output)
df.to_csv(csv_filename, index=False)
csv_output = {'Path x-coordinates': x_true, 'Path y-coordinates': y_true}
df = pd.DataFrame(csv_output)
df.to_csv(csv_filename[:-4] + '_path.csv', index=False)
def random_path_test():
rpg = RandomPathGenerator()
x_true, y_true, t, vel = rpg.get_random_path()
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
pid = StanleyPID(k_crosstrack={
'P': 5,
'I': 0.1,
'D': 0.5
},
k_heading={
'P': -0.5,
'I': 0,
'D': 0
})
x = []
y = []
delta = []
th1 = []
th2 = []
ct_err = []
hd_err = []
for i in range(0, len(t)):
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, err_int, err_diff = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x.append(xt)
y.append(yt)
delta.append(deltat)
th1.append(th1t)
th2.append(th2t)
ct_err.append(err[0])
hd_err.append(err[1])
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.show()
plt.plot(t, ct_err)
plt.show()
plt.plot(t, hd_err)
plt.show()
def pid_test():
'''
Test for the Stanley PID controller. Takes the path data from a Simpack CSV
and uses it as the desired path for the controller to follow. Plots the
resulting driven path against the Simpack-recorded path.
'''
# x_true,y_true,t,vel,steer_angle = read_path_from_original_simpack_csv('/Users/Zeke/Documents/MATLAB/Model Validation/P4_TCO_Sleeper__FE17_Trailer/DLC/Benchmark_DLC_31ms_pandas.csv')
x_true, y_true, t, vel, steer_angle = read_path_from_original_simpack_csv(
'/Users/Zeke/Documents/MATLAB/Model Validation/P4_TCO_Sleeper__FE17_Trailer/SScorner_var_spd/left/Benchmark_SScorner_80m_left_pandas.csv'
)
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
pid = StanleyPID(k_crosstrack={
'P': 5,
'I': 0.1,
'D': 0.1
},
k_heading={
'P': -0.5,
'I': 0,
'D': 0
})
x = []
y = []
delta = []
th1 = []
th2 = []
for i in range(0, len(t)):
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, err_int, err_diff = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x.append(xt)
y.append(yt)
delta.append(deltat)
th1.append(th1t)
th2.append(th2t)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.show()
def train_network(network, k_crosstrack, k_heading):
rpg = RandomPathGenerator()
x_true, y_true, t, vel = rpg.get_harder_path()
xt1 = x_true
yt1 = y_true
t12 = t
vel1 = vel
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
controller = NN2Control()
pid = StanleyPID(k_crosstrack=k_crosstrack, k_heading=k_heading)
pid2 = StanleyPID(k_crosstrack=k_crosstrack, k_heading=k_heading)
learning_rate = 0.01
x = []
y = []
xp = []
yp = []
delta = []
th1 = []
th2 = []
th1t = 0
th2t = 0
running_loss = 0
#do some random stuff
ego2 = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
for i in range(0, len(t)):
network = network.float()
state = ego.convert_world_state_to_front()
state1 = ego2.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t, network)
ctrl_delta_pid, ctrl_vel_pid, err_pid, interr_pid, differr_pid = pid.calc_steer_control(
t[i], state1, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x1, y1, delt, tha, thb = ego2.simulate_timestep(
[ctrl_vel_pid, ctrl_delta_pid])
xp.append(x1)
yp.append(y1)
x.append(xt)
y.append(yt)
delta.append(deltat)
th1.append(th1t)
th2.append(th2t)
inputs = np.concatenate((err, ctrl_vel, interr[0], differr[0]),
axis=None)
inputs = np.append(inputs, th1t - th2t)
network_input = torch.tensor(inputs)
network = network.double()
out = network(network_input)
#print(running_loss)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g--')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.xlabel('X location, (m)')
plt.ylabel('Y Location, (m)')
plt.show()
running_loss = 0
x = []
y = []
xp = []
yp = []
delta = []
th1 = []
th2 = []
pl = 0
plot_loss = np.zeros([5, 100])
loss_time = list(range(0, 20000, 200))
MSE = []
MSE1 = 0
th1t = 0
th2t = 0
temp_controller1 = pickle.loads(pickle.dumps(network))
temp_controller2 = pickle.loads(pickle.dumps(network))
temp_controller3 = pickle.loads(pickle.dumps(network))
temp_controller4 = pickle.loads(pickle.dumps(network))
networks = [
network, temp_controller1, temp_controller2, temp_controller3,
temp_controller4
]
for j in range(5):
pl = 0
for i in range(20000):
nwork = networks[j]
pid_list = pid.control_from_random_error()
input1 = pid_list[0:3]
input2 = pid_list[4:]
input1 = np.concatenate((input1, input2), axis=0)
input1 = np.append(input1, np.pi * np.random.random() - np.pi / 2)
#print(input1)
target1 = pid_list[3]
network_input = torch.tensor(input1)
#print(network_input)
network_target = torch.tensor(target1)
network_target = network_target.double()
nwork = nwork.double()
#print(network_input)
out = nwork(network_input)
nwork.zero_grad()
criterion = nn.MSELoss()
loss = criterion(out, network_target)
loss.backward()
pl += loss.item()
if (i % 200 == 199):
print(int((i + 1) / 200 - 1))
plot_loss[j][int((i + 1) / 200 - 1)] = (pl / 200)
pl = 0
running_loss += loss.item()
for f in nwork.parameters():
f.data.sub_(f.grad.data * learning_rate)
#print(running_loss)
plot_loss_avg = np.average(plot_loss, axis=0)
plot_loss_std = np.std(plot_loss, axis=0)
#plt.plot(loss_time,plot_loss)
plt.errorbar(loss_time,
plot_loss_avg,
yerr=plot_loss_std,
marker='s',
capsize=5)
plt.xlabel('Iteration Number')
plt.ylabel('Average Loss of Past 200 Iterations, (rad)')
plt.show()
#follow the path
running_loss = 0
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
ego2 = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
controller = NN2Control()
pid = StanleyPID(k_crosstrack=k_crosstrack, k_heading=k_heading)
pid2 = StanleyPID(k_crosstrack=k_crosstrack, k_heading=k_heading)
pl = 0
plot_loss = []
for i in range(0, len(t)):
network = network.float()
state = ego.convert_world_state_to_front()
state1 = ego2.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t, network)
ctrl_delta_pid, ctrl_vel_pid, err_pid, interr_pid, differr_pid = pid2.calc_steer_control(
t[i], state, x_true, y_true, vel)
ctrl_delta_pid1, ctrl_vel_pid1, err_pid1, interr_pid1, differr_pid1 = pid.calc_steer_control(
t[i], state1, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x1, y1, delt, tha, thb = ego2.simulate_timestep(
[ctrl_vel_pid1, ctrl_delta_pid1])
xp.append(x1)
yp.append(y1)
x.append(xt)
y.append(yt)
delta.append(deltat)
th1.append(th1t)
th2.append(th2t)
inputs = np.concatenate((err, ctrl_vel, interr[0], differr[0]),
axis=None)
inputs = np.append(inputs, th1t - th2t)
network_input = torch.tensor(inputs)
network_target = torch.tensor(ctrl_delta_pid)
network_target = network_target.double()
network = network.double()
out = network(network_input)
network.zero_grad()
criterion = nn.MSELoss()
loss = criterion(out, network_target)
loss.backward()
if (i % len(t) / 20) == (len(t) / 20 - 1):
plot_loss.append(pl / len(t) * 20)
loss_time.append(i)
pl = 0
running_loss += loss.item()
for f in network.parameters():
f.data.sub_(f.grad.data * learning_rate)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g--')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.xlabel('X location, (m)')
plt.ylabel('Y Location, (m)')
plt.show()
def main():
network = train_nn_from_pid()
#Read in the benchmark paths that we will use
Benchmark1 = pd.read_csv('Benchmark_DLC_31ms_reduced.csv',
sep=',',
header=0)
Benchmark1 = Benchmark1.values
Benchmark2 = pd.read_csv('Benchmark_SScorner_80m_left_reduced.csv',
sep=',',
header=0)
Benchmark2 = Benchmark2.values
Benchmark3 = pd.read_csv('Benchmark_SScorner_500m_left_25ms_reduced.csv',
sep=',',
header=0)
Benchmark3 = Benchmark3.values
'''$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
for k in range(5):
thing_range=list(range(50))
np.random.shuffle(thing_range)
total_loss=0
for i in thing_range:
running_loss=0
for j in range(len(PID_Data)):
#print(np.shape(input1))
#print(network.parameters())
#print(network.fc1.weight.data)
#print(network.fc3.weight.data)
#print(j)
network_input=torch.tensor(input1[j])
#print(network_input)
network_target=torch.tensor(target1[j])
network_target=network_target.double()
network= network.double()
out=network(network_input)
network.zero_grad()
criterion = nn.MSELoss()
loss = criterion(out, network_target)
loss.backward()
running_loss += loss.item()
#print(out.data,network_target.data, out.data-network_target.data)
#print(loss.item())
for f in network.parameters():
f.data.sub_(f.grad.data * learning_rate)
print('[%5d] loss: %.3f' % (i + 1, running_loss))
#if running_loss >= 5:
#input('press enter')
total_loss+=running_loss
PID_Data=pd.read_csv('random_path_pid_more_output_'+str(i)+'.csv',sep=',',header=0)
#print(PID_Data)
PID_Data=PID_Data.values
np.random.shuffle(PID_Data)
input1=PID_Data[:,0:2]
input2=PID_Data[:,4:]
input1=np.concat enate((input1,input2),axis=1)
#print(input1)
target1=PID_Data[:,3]
print('total loss this set: ', total_loss)
#print('[%5d] loss: %.3f' %
#(i + 1, running_loss))
'''
#Train the network until it is sufficient, asking the human operator for input on whether the point it reached is good enough
'''
network=network.float()
for i in range(10):
train_network(network)
a=input('is this good enough?')
if a=='1':
break
'''
'''
test_suite.basic_fitness_comparison(network)
test_suite.trailer_length_variation_test(network)
test_suite.trailer_mass_variation_test(network)
test_suite.trailer_stiffness_variation_test(network)
test_suite.noisy_signal_test(network)
test_suite.initial_displacement_test(network)
'''
#Initialize varriables to run the first benchmark test on the PID mimicking controller
network = network.float()
controller = NN2Control()
x_true = Benchmark1[:, 0]
y_true = Benchmark1[:, 1]
t = Benchmark1[:, 2]
vel = Benchmark1[:, 3]
xp = []
yp = []
x = []
y = []
pid = StanleyPID()
#Run the same benchmark on both the PID controller and the PID mimicking network and compare the two
for i in range(2):
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
print('controller: ', i)
th1t = 0
th2t = 0
th1 = []
th2 = []
x_truck = []
y_truck = []
for j in range(len(t)):
if i == 1:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
xp.append(xt)
yp.append(yt)
else:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t, network)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
x.append(xt)
y.append(yt)
if i == 1:
pid_fitness, CTerr = calc_off_tracking(x_truck, y_truck, th1, th2,
ego.P, x_true, y_true)
else:
controller_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
print('Benchmark 1 PID fitness: ', pid_fitness)
print('Benchmark 1 controller fitness: ', controller_fitness)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g:')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.xlabel('X location, (m)')
plt.ylabel('Y Location, (m)')
plt.show()
#send the pid mimicking controller to the evolutionary algorithm
#print('bias value before evo: ', network.fc3.bias.data)
evolution = EvolutionaryAlgorithm(network)
for i in range(1000):
print(i)
evolution.iterate()
#every 20 steps, run a benchmark on the best controller in the system to see how it is progressing
if i % 100 == 0:
'''
Fc1=network.fc1.weight.data.numpy()
Fc2=network.fc2.weight.data.numpy()
Fc3=network.fc3.weight.data.numpy()
Evo1=evolution.controllers[evolution.best_controller_idx].fc1.weight.data.numpy()
Evo2=evolution.controllers[evolution.best_controller_idx].fc2.weight.data.numpy()
Evo3=evolution.controllers[evolution.best_controller_idx].fc3.weight.data.numpy()
print((Fc1-Evo1))
print(np.linalg.norm((Fc1-Evo1)))
print((Fc2-Evo2))
print(np.linalg.norm((Fc2-Evo2)))
print((Fc3-Evo3))
print(np.linalg.norm((Fc3-Evo3)))
Fc1b=network.fc1.bias.data.numpy()
Fc2b=network.fc2.bias.data.numpy()
Fc3b=network.fc3.bias.data.numpy()
Evo1b=evolution.controllers[evolution.best_controller_idx].fc1.bias.data.numpy()
Evo2b=evolution.controllers[evolution.best_controller_idx].fc2.bias.data.numpy()
Evo3b=evolution.controllers[evolution.best_controller_idx].fc3.bias.data.numpy()
print((Fc1b-Evo1b))
print(np.linalg.norm((Fc1b-Evo1b)))
print((Fc2b-Evo2b))
print(np.linalg.norm((Fc2b-Evo2b)))
print((Fc3b-Evo3b))
print(np.linalg.norm((Fc3b-Evo3b)))
'''
controller = NN2Control()
x_true = Benchmark1[:, 0]
y_true = Benchmark1[:, 1]
t = Benchmark1[:, 2]
vel = Benchmark1[:, 3]
x = []
y = []
xp = []
yp = []
x_truck = []
y_truck = []
th1t = 0
th2t = 0
th1 = []
th2 = []
pid = StanleyPID()
for i in range(2):
ego = EgoSim(sim_timestep=t[1] - t[0],
world_state_at_front=True)
print('controller: ', i)
th1t = 0
th2t = 0
th1 = []
th2 = []
x_truck = []
y_truck = []
for j in range(len(t)):
if i == 1:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
xp.append(xt)
yp.append(yt)
else:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t,
evolution.controllers[
evolution.best_controller_idx])
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
x.append(xt)
y.append(yt)
if i == 1:
pid_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
else:
controller_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
print('Benchmark 1 PID fitness: ', pid_fitness)
print('Benchmark 1 controller fitness: ', controller_fitness)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g:')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.xlabel('X location, (m)')
plt.ylabel('Y Location, (m)')
plt.show()
#Initialize varriables to run the first benchmark test
controller = NN2Control()
x_true = Benchmark1[:, 0]
y_true = Benchmark1[:, 1]
t = Benchmark1[:, 2]
vel = Benchmark1[:, 3]
x = []
y = []
xp = []
yp = []
x_truck = []
y_truck = []
th1t = 0
th2t = 0
th1 = []
th2 = []
pid = StanleyPID()
#after the network has finished its evolutionary training, check it the first benchmark
for i in range(2):
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
print('controller: ', i)
th1t = 0
th2t = 0
th1 = []
th2 = []
x_truck = []
y_truck = []
for j in range(len(t)):
if i == 1:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
xp.append(xt)
yp.append(yt)
else:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t,
evolution.controllers[evolution.best_controller_idx])
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
x.append(xt)
y.append(yt)
if i == 1:
pid_fitness, CTerr = calc_off_tracking(x_truck, y_truck, th1, th2,
ego.P, x_true, y_true)
else:
controller_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
print('Benchmark 1 PID fitness: ', pid_fitness)
print('Benchmark 1 controller fitness: ', controller_fitness)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g:')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.xlabel('X location, (m)')
plt.ylabel('Y Location, (m)')
plt.show()
#Initialize varriables to run the second benchmark test on the controller trained on the
x_true = Benchmark2[:, 0]
y_true = Benchmark2[:, 1]
t = Benchmark2[:, 2]
vel = Benchmark2[:, 3]
x_truck = []
y_truck = []
th1t = 0
th2t = 0
th1 = []
th2 = []
pid = StanleyPID()
x = []
y = []
xp = []
yp = []
#check it the second benchmark
for i in range(2):
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
print('controller: ', i)
th1t = 0
th2t = 0
th1 = []
th2 = []
x_truck = []
y_truck = []
for j in range(len(t)):
if i == 1:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
xp.append(xt)
yp.append(yt)
else:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t,
evolution.controllers[evolution.best_controller_idx])
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
x.append(xt)
y.append(yt)
#inputs=np.concatenate((err,ctrl_vel,interr,differr),axis=None)
#network_input=torch.tensor(inputs)
#out=self.controllers[i](network_input)
#x.append(xt); y.append(yt); delta.append(deltat); th1.append(th1t); th2.append(th2t)
if i == 1:
pid_fitness, CTerr = calc_off_tracking(x_truck, y_truck, th1, th2,
ego.P, x_true, y_true)
else:
controller_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
print('Benchmark 2 PID fitness: ', pid_fitness)
print('Benchmark 2 controller fitness: ', controller_fitness)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g:')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.xlabel('X location, (m)')
plt.ylabel('Y Location, (m)')
plt.show()
x_true = Benchmark3[:, 0]
y_true = Benchmark3[:, 1]
t = Benchmark3[:, 2]
vel = Benchmark3[:, 3]
x = []
y = []
xp = []
yp = []
x_truck = []
y_truck = []
th1t = 0
th2t = 0
th1 = []
th2 = []
pid = StanleyPID()
for i in range(2):
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
print('controller: ', i)
th1t = 0
th2t = 0
th1 = []
th2 = []
x_truck = []
y_truck = []
for j in range(len(t)):
if i == 1:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
xp.append(xt)
yp.append(yt)
else:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t,
evolution.controllers[evolution.best_controller_idx])
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
x.append(xt)
y.append(yt)
#inputs=np.concatenate((err,ctrl_vel,interr,differr),axis=None)
#network_input=torch.tensor(inputs)
#out=self.controllers[i](network_input)
#x.append(xt); y.append(yt); delta.append(deltat); th1.append(th1t); th2.append(th2t)
if i == 1:
pid_fitness, CTerr = calc_off_tracking(x_truck, y_truck, th1, th2,
ego.P, x_true, y_true)
else:
controller_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
print('Benchmark 3 PID fitness: ', pid_fitness)
print('Benchmark 3 controller fitness: ', controller_fitness)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g:')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.xlabel('X location, (m)')
plt.ylabel('Y Location, (m)')
plt.show()
#
#controller=NNControl()
network = evolution.controllers[evolution.best_controller_idx]
test_suite.basic_fitness_comparison(network)
test_suite.trailer_length_variation_test(network)
test_suite.trailer_mass_variation_test(network)
test_suite.trailer_stiffness_variation_test(network)
test_suite.noisy_signal_test(network)
test_suite.initial_displacement_test(network)
#controller.calc_steer_control(t,state,path_x,path_y,path_vel,network)
#a=network.parameters()
#print(a)
'''
def test_network_on_benchmark(network, Benchmark):
#Initialize varriables to run the first benchmark test on the PID mimicking controller
network = network.float()
controller = NN2Control()
x_true = Benchmark[:, 0]
y_true = Benchmark[:, 1]
t = Benchmark[:, 2]
vel = Benchmark[:, 3]
xp = []
yp = []
x = []
y = []
pid = StanleyPID()
#Run the same benchmark on both the PID controller and the PID mimicking network and compare the two
for i in range(2):
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
print('controller: ', i)
th1t = 0
th2t = 0
th1 = []
th2 = []
x_truck = []
y_truck = []
for j in range(len(t)):
if i == 1:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
xp.append(xt)
yp.append(yt)
else:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t, network)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
x.append(xt)
y.append(yt)
if i == 1:
pid_fitness, CTerr = calc_off_tracking(x_truck, y_truck, th1, th2,
ego.P, x_true, y_true)
else:
controller_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
print('Benchmark PID fitness: ', pid_fitness)
print('Benchmark controller fitness: ', controller_fitness)
plt.plot(x, y)
plt.plot(x_true, y_true, 'r--')
plt.plot(xp, yp, 'g--')
plt.legend(['Network Performance', 'True Path', 'PID Performance'])
plt.show()
def evaluate_fitness(self):
'''
Evaluates and returns the fitness of all controllers in pool
'''
rpg = RandomPathGenerator()
controller = NN2Control()
x_true, y_true, t, vel = rpg.get_harder_path(end_time=10)
x_truck = []
y_truck = []
self.controller_fitness = np.zeros(len(self.controllers))
th1t = 0
th2t = 0
th1 = []
th2 = []
pid = StanleyPID()
for i in range(len(self.controllers) + 1):
#print(t)
#print(x_true)
#print(y_true)
#print(vel)
ego = EgoSim(sim_timestep=t[1] - t[0], world_state_at_front=True)
controller = NN2Control()
#print('controller: ', i)
th1t = 0
th2t = 0
th1 = []
th2 = []
x_truck = []
y_truck = []
for j in range(len(t)):
if i == len(self.controllers):
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = pid.calc_steer_control(
t[i], state, x_true, y_true, vel)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
else:
state = ego.convert_world_state_to_front()
ctrl_delta, ctrl_vel, err, interr, differr = controller.calc_steer_control(
t[i], state, x_true, y_true, vel, th1t - th2t,
self.controllers[i])
#print(ctrl_delta)
xt, yt, deltat, th1t, th2t = ego.simulate_timestep(
[ctrl_vel, ctrl_delta])
x_truck.append(xt)
y_truck.append(yt)
th1.append(th1t)
th2.append(th2t)
#inputs=np.concatenate((err,ctrl_vel,interr,differr),axis=None)
#network_input=torch.tensor(inputs)
#out=self.controllers[i](network_input)
#x.append(xt); y.append(yt); delta.append(deltat); th1.append(th1t); th2.append(th2t)
if i == len(self.controllers):
self.pid_fitness, CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)
else:
self.controller_fitness[i], CTerr = calc_off_tracking(
x_truck, y_truck, th1, th2, ego.P, x_true, y_true)