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