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