def mpcPlot(self): dy = Dynamics.VehicleDynamics() ref = dy.reference_trajectory( Numpy2Torch(self.state_history[:, -1], self.state_history[:, -1].shape)) self.state_r_history = self.state_r_history.reshape([-1, 5]) plt.figure(1) plt.plot(self.state_history[:, -1], self.state_history[:, 0], label="trajectory") # plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$") # plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference") plt.plot(self.state_history[:, -1], ref[:, 0], label="reference") plt.legend(loc="upper right") plt.figure(2) plt.plot(self.state_history[:, -1], self.state_history[:, 2], label="trajectory") # plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$") # plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference") plt.plot(self.state_history[:, -1], ref[:, 2], label="reference") plt.legend(loc="upper right") plt.figure(3) plt.plot(self.state_history[0:-1, -1], self.control_history) plt.show()
def mpcSolution(self): # initialize state model statemodel_plt = Dynamics.VehicleDynamics() state = self.initial_state # state = torch.tensor([[0.0, 0.0, self.psi_init, 0.0, 0.0]]) state.requires_grad_(False) x_ref = statemodel_plt.reference_trajectory(state[:, -1]) state_r = state.detach().clone() # relative state state_r[:, 0:4] = state_r[:, 0:4] - x_ref self.state_history = state.detach().numpy() plot_length = self.config.SIMULATION_STEPS self.control_history = [] self.state_r_history = state_r cal_time = 0 plt.figure(0) for i in range(plot_length): x = state_r.tolist()[0] time_start = time.time() temp, control = self.solver.mpcSolver(x, self.config.NP) plt.plot(temp[:, -1], temp[:, 0]) cal_time += time.time() - time_start u = Numpy2Torch(control[0], (-1, self.config.ACTION_DIM)) state, state_r = step_relative(statemodel_plt, state, u) self.state_history = np.append(self.state_history, state.detach().numpy(), axis=0) self.control_history = np.append(self.control_history, u.detach().numpy()) self.state_r_history = np.append(self.state_history, state_r.detach().numpy()) print("MPC calculating time: {:.3f}".format(cal_time) + "s") self.mpcSaveTraj()
# control_history = np.empty([0, 1]) # time_init = time.time() # for i in range(config.NP_TOTAL): # state, control = solver.mpc_solver(x, 60) # x = state[1] # u = control[0] # state_history = np.append(state_history, x) # control_history = np.append(control_history, u) # x = x.tolist() # print("steps:{:3d}".format(i) + " | state: " + str(x)) # print("MPC calculating time: {:.3f}".format(time.time() - time_init) + "s") # state_history = state_history.reshape(-1,config.DYNAMICS_DIM) # # np.savetxt(os.path.join(log_dir, 'structured_MPC_state.txt'), state_history) # # np.savetxt(os.path.join(log_dir, 'structured_MPC_control.txt'), control_history) statemodel_plt = Dynamics.VehicleDynamics() if config.tire_model == 'Fiala': state = torch.tensor([[1.0, 0.0, config.psi_init, 0.0, config.u, 0.0]]) else: state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]]) state.requires_grad_(False) x_ref = statemodel_plt.reference_trajectory(state[:, -1]) state_r = state.detach().clone() state_r[:, 0:4] = state_r[:, 0:4] - x_ref state_history = state.detach().numpy() x = np.array([0.]) plot_length = 300 control_history = [] state_r_history = state_r cal_time = 0 plt.figure()
def simulation(methods, log_dir, simu_dir): policy = Actor(S_DIM, A_DIM) value = Critic(S_DIM, A_DIM) config = DynamicsConfig() solver = Solver() load_dir = log_dir policy.load_parameters(load_dir) value.load_parameters(load_dir) statemodel_plt = Dynamics.VehicleDynamics() plot_length = config.SIMULATION_STEPS # initial_state = torch.tensor([[0.5, 0.0, config.psi_init, 0.0, 0.0]]) # baseline = Baseline(initial_state, simu_dir) # baseline.mpcSolution() # baseline.openLoopSolution() # Open-loop reference x_init = [0.0, 0.0, config.psi_init, 0.0, 0.0] op_state, op_control = solver.openLoopMpcSolver(x_init, config.NP_TOTAL) np.savetxt(os.path.join(simu_dir, 'Open_loop_control.txt'), op_control) for method in methods: cal_time = 0 state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]]) # state = torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0]]) state.requires_grad_(False) # x_ref = statemodel_plt.reference_trajectory(state[:, -1]) x_ref = statemodel_plt.reference_trajectory(state[:, -1]) state_r = state.detach().clone() state_r[:, 0:4] = state_r[:, 0:4] - x_ref state_history = state.detach().numpy() control_history = [] print('\nCALCULATION TIME:') for i in range(plot_length): if method == 'ADP': time_start = time.time() u = policy.forward(state_r[:, 0:4]) cal_time += time.time() - time_start elif method == 'MPC': x = state_r.tolist()[0] time_start = time.time() _, control = solver.mpcSolver(x, config.NP) # todo:retreve cal_time += time.time() - time_start u = np.array(control[0], dtype='float32').reshape(-1, config.ACTION_DIM) u = torch.from_numpy(u) else: u = np.array(op_control[i], dtype='float32').reshape(-1, config.ACTION_DIM) u = torch.from_numpy(u) state, state_r = step_relative(statemodel_plt, state, u) # state_next, deri_state, utility, F_y1, F_y2, alpha_1, alpha_2 = statemodel_plt.step(state, u) # state_r_old, _, _, _, _, _, _ = statemodel_plt.step(state_r, u) # state_r = state_r_old.detach().clone() # state_r[:, [0, 2]] = state_next[:, [0, 2]] # x_ref = statemodel_plt.reference_trajectory(state_next[:, -1]) # state_r[:, 0:4] = state_r[:, 0:4] - x_ref # state = state_next.clone().detach() # s = state_next.detach().numpy() state_history = np.append(state_history, state.detach().numpy(), axis=0) control_history = np.append(control_history, u.detach().numpy()) if method == 'ADP': print(" ADP: {:.3f}".format(cal_time) + "s") np.savetxt(os.path.join(simu_dir, 'ADP_state.txt'), state_history) np.savetxt(os.path.join(simu_dir, 'ADP_control.txt'), control_history) elif method == 'MPC': print(" MPC: {:.3f}".format(cal_time) + "s") np.savetxt(os.path.join(simu_dir, 'structured_MPC_state.txt'), state_history) np.savetxt(os.path.join(simu_dir, 'structured_MPC_control.txt'), control_history) else: np.savetxt(os.path.join(simu_dir, 'Open_loop_state.txt'), state_history) adp_simulation_plot(simu_dir) plot_comparison(simu_dir, methods)
LR_V = 3e-3 # learning rate of value net # tasks TRAIN_FLAG = 1 LOAD_PARA_FLAG = 0 SIMULATION_FLAG = 1 # Set random seed np.random.seed(0) torch.manual_seed(0) # initialize policy and value net, model of vehicle dynamics config = GeneralConfig() policy = Actor(config.STATE_DIM, config.ACTION_DIM, lr=LR_P) value = Critic(config.STATE_DIM, 1, lr=LR_V) vehicleDynamics = Dynamics.VehicleDynamics() state_batch = vehicleDynamics.initialize_state() writer = SummaryWriter() # Training iteration_index = 0 if LOAD_PARA_FLAG == 1: print( "********************************* LOAD PARAMETERS *********************************" ) # load pre-trained parameters load_dir = "./Results_dir/2020-10-09-14-42-10000" policy.load_parameters(load_dir) value.load_parameters(load_dir) if TRAIN_FLAG == 1:
def plot_comparison(simu_dir, methods): ''' Plot comparison figure among ADP, MPC & open-loop solution. Trajectory, tracking error and control signal plot Parameters ---------- picture_dir: string location of figure saved. ''' num_methods = len(methods) picture_dir = simu_dir + "/Figures" config = DynamicsConfig() trajectory_data = [] dy = Dynamics.VehicleDynamics() if os.path.exists(os.path.join(simu_dir, 'structured_MPC_state.txt')) == 0: print('No comparison state data!') else: if 'MPC' in methods: mpc_state = np.loadtxt( os.path.join(simu_dir, 'structured_MPC_state.txt')) mpc_trajectory = (mpc_state[:, 4], mpc_state[:, 0]) trajectory_data.append(mpc_trajectory) if 'ADP' in methods: adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt')) adp_trajectory = (adp_state[:, 4], adp_state[:, 0]) trajectory_data.append(adp_trajectory) if 'OP' in methods: open_loop_state = np.loadtxt( os.path.join(simu_dir, 'Open_loop_state.txt')) open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:, 0]) trajectory_data.append(open_loop_trajectory) myplot(trajectory_data, num_methods, "xy", fname=os.path.join(picture_dir, 'trajectory.png'), xlabel="longitudinal position [m]", ylabel="Lateral position [m]", legend=methods, legend_loc="lower left") heading_angle = [] if 'MPC' in methods: mpc_state = np.loadtxt( os.path.join(simu_dir, 'structured_MPC_state.txt')) mpc_trajectory = (mpc_state[:, 4], 180 / np.pi * mpc_state[:, 2]) heading_angle.append(mpc_trajectory) if 'ADP' in methods: adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt')) adp_trajectory = (adp_state[:, 4], 180 / np.pi * adp_state[:, 2]) heading_angle.append(adp_trajectory) if 'OP' in methods: open_loop_state = np.loadtxt( os.path.join(simu_dir, 'Open_loop_state.txt')) open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:, 2]) heading_angle.append(open_loop_trajectory) myplot(heading_angle, num_methods, "xy", fname=os.path.join(picture_dir, 'trajectory_heading_angle.png'), xlabel="longitudinal position [m]", ylabel="Heading angle [degree]", legend=methods, legend_loc="lower left") error_data = [] print('\nMAX TRACKING ERROR:') print(' MAX LATERAL POSITION ERROR:') if 'MPC' in methods: mpc_ref = dy.reference_trajectory( Numpy2Torch(mpc_state[:, 4], mpc_state[:, 4].shape)).numpy() mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - mpc_ref[:, 0]) print(' Max MPC lateral error: {:0.3E}m'.format( float(max(abs(mpc_state[:, 0] - mpc_ref[:, 0]))))) error_data.append(mpc_error) if 'ADP' in methods: adp_ref = dy.reference_trajectory( Numpy2Torch(adp_state[:, 4], adp_state[:, 4].shape)).numpy() adp_error = (adp_state[:, 4], adp_state[:, 0] - adp_ref[:, 0]) print(' Max ADP lateral error: {:0.3E}m'.format( float(max(abs(adp_state[:, 0] - adp_ref[:, 0]))))) error_data.append(adp_error) if 'OP' in methods: open_loop_error = (open_loop_state[:, 4], open_loop_state[:, 0] - config.a_curve * np.sin(config.k_curve * open_loop_state[:, 4])) error_data.append(open_loop_error) myplot(error_data, num_methods, "xy", fname=os.path.join(picture_dir, 'trajectory_error.png'), xlabel="longitudinal position error [m]", ylabel="Lateral position [m]", legend=methods, legend_loc="upper left") # mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - config.a_curve * np.sin(config.k_curve * mpc_state[:, 4])) # open_loop_error = (open_loop_state[:, 4], open_loop_state[:, 0] - config.a_curve * np.sin(config.k_curve * open_loop_state[:, 4])) # adp_error = (adp_state[:, 4], 1 * (adp_state[:, 0] - config.a_curve * np.sin(config.k_curve * adp_state[:, 4]))) # TODO: safe delete #error_data = [mpc_error, adp_error, open_loop_error] # myplot(error_data, 3, "xy", # fname=os.path.join(picture_dir,'trajectory_error.png'), # xlabel="longitudinal position [m]", # ylabel="Lateral position error [m]", # legend=["MPC", "ADP", "Open-loop"], # legend_loc="lower left" # ) psi_error_data = [] print(' MAX YAW ANGLE ERROR:') if 'MPC' in methods: mpc_psi_error = (mpc_state[:, 4], 180 / np.pi * (mpc_state[:, 2] - mpc_ref[:, 2])) print(' MPC: {:0.3E} degree'.format( float(max(abs(180 / np.pi * (mpc_state[:, 2] - mpc_ref[:, 2])))))) psi_error_data.append(mpc_psi_error) if 'ADP' in methods: adp_psi_error = (adp_state[:, 4], 180 / np.pi * (adp_state[:, 2] - adp_ref[:, 2])) print(' ADP: {:0.3E} degree'.format( float(max(abs(180 / np.pi * (adp_state[:, 2] - adp_ref[:, 2])))))) psi_error_data.append(adp_psi_error) if 'OP' in methods: open_loop_psi_error = ( open_loop_state[:, 4], 180 / np.pi * (open_loop_state[:, 2] - np.arctan(config.a_curve * config.k_curve * np.cos(config.k_curve * open_loop_state[:, 4])))) psi_error_data.append(open_loop_psi_error) myplot(psi_error_data, num_methods, "xy", fname=os.path.join(picture_dir, 'head_angle_error.png'), xlabel="longitudinal position [m]", ylabel="head angle error [degree]", legend=["MPC", "ADP", "Open-loop"], legend_loc="lower left") control_plot_data = [] if 'MPC' in methods: mpc_control = np.loadtxt( os.path.join(simu_dir, 'structured_MPC_control.txt')) mpc_control_tuple = (mpc_state[1:, 4], 180 / np.pi * mpc_control) control_plot_data.append(mpc_control_tuple) if 'ADP' in methods: adp_control = np.loadtxt(os.path.join(simu_dir, 'ADP_control.txt')) adp_control_tuple = (adp_state[1:, 4], 180 / np.pi * adp_control) control_plot_data.append(adp_control_tuple) if 'OP' in methods: open_loop_control = np.loadtxt( os.path.join(simu_dir, 'Open_loop_control.txt')) open_loop_control_tuple = (open_loop_state[1:, 4], 180 / np.pi * open_loop_control) control_plot_data.append(open_loop_control_tuple) myplot(control_plot_data, 3, "xy", fname=os.path.join(picture_dir, 'control.png'), xlabel="longitudinal position [m]", ylabel="steering angle [degree]", legend=["MPC", "ADP", "Open-loop"], legend_loc="upper left") if 'OP' in methods: y_avs_error = [] for [i, d] in enumerate(error_data): y_avs_error.append(np.mean(np.abs(d[1]))) print("Tracking error of lateral position:") print("MPC:{:.3e} | ".format(y_avs_error[0]) + "ADP:{:.3e} | ".format(y_avs_error[1]) + "Open-loop:{:.3e} | ".format(y_avs_error[2])) psi_avs_error = [] for [i, d] in enumerate(psi_error_data): psi_avs_error.append(np.mean(np.abs(d[1]))) print("Tracking error of heading angle:") print("MPC:{:.3e} | ".format(psi_avs_error[0]) + "ADP:{:.3e} | ".format(psi_avs_error[1]) + "Open-loop:{:.3e} | ".format(psi_avs_error[2])) mpc_control_error = mpc_control - open_loop_control adp_control_error = adp_control - open_loop_control print("Control error:") print("MPC:{:.3e} | ".format(np.mean(np.abs(mpc_control_error))) + "ADP:{:.3e} | ".format(np.mean(np.abs(adp_control_error))))