diff --git a/.gitignore b/.gitignore index f717486..1161bd9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,7 @@ +Outputs/ +.vscode/ +.vs/ + # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] diff --git a/MAIN_hybridMFC/MAIN_hybrid_mfc.py b/MAIN_hybridMFC/MAIN_hybrid_mfc.py index 76c9707..5a59158 100644 --- a/MAIN_hybridMFC/MAIN_hybrid_mfc.py +++ b/MAIN_hybridMFC/MAIN_hybrid_mfc.py @@ -8,6 +8,7 @@ from itertools import product from functools import partial from IPython.core import display as ICD +from scipy.interpolate import interp1d pathRoot = os.path.abspath(__file__ + "/../../") sys.path.append(pathRoot) @@ -22,23 +23,35 @@ def mfc_main( hyd_mode, veh_load, driver_style, - driver_style_dc, + gs_style, res_path ): - car_info, acc_curve, dec_curve, veh_model_speed, veh_model_acc, veh_model_dec, veh_max_speed \ + car_info, ap_curve, dp_curve, veh_model_speed, veh_model_acc, veh_model_dec, veh_model_acc_max, veh_max_speed, gs_th \ = msim.mfc_curves( car, car_id, hyd_mode, - veh_load + veh_load, + gs_style, ) + + plt.plot(veh_model_speed, veh_model_acc, '--', label='($a_{ap, gs}$)') + plt.plot(veh_model_speed, veh_model_acc_max, label='($a_{ap, max}$)') + plt.legend() + plt.xlabel(r'Veh. spd ($m/s$)') + plt.ylabel(r'Veh. accel ($m/s^2$)') + plt.grid() + plt.tight_layout() + plt.savefig(os.path.join(res_path, 'ap_gs_max_curves_comp.png')) + plt.close() df = pd.DataFrame() df['Spd [m/s]'] = veh_model_speed + df['AccelMax [m/s2]'] = veh_model_acc_max df['Accel [m/s2]'] = veh_model_acc df['Decel [m/s2]'] = veh_model_dec - df.to_csv(os.path.join(res_path, hyd_mode+'_MFC_curves.csv'), index=False) + df.to_csv(os.path.join(res_path, 'ap_dp_curves.csv'), index=False) msim.plt_exp_val_spd_accel( pathRoot, @@ -52,23 +65,140 @@ def mfc_main( res_path ) - msim.plt_accel_scenario( - driver_style, - acc_curve, - dec_curve, - veh_max_speed, - car_info, - res_path - ) + (f_des, v0, duration, cycle_des) = [ + (interp1d( # Varying speed limits scenario + [0, 210, 700, 1050, 1400, 3150, 4550, 5250], # Position (m) + [20, 20, 30, 5, 25, veh_max_speed, 15, 0], # Desired speed (m/s) + kind="next", fill_value="extrapolate",), 0, 250, 'var'), + (interp1d( # Acceleration scenario + [0, 5250], # Position (m) + [veh_max_speed, veh_max_speed], # Desired speed (m/s) + kind="next", fill_value="extrapolate",), 0, 100, 'acc'), + (interp1d( + [0, 5250], # Position (m) + [0, 0], # Desired speed (m/s) + kind="next", fill_value="extrapolate",), veh_max_speed, 20, 'dec'), + ][2] ### Changable - msim.plt_accel_scenario_dc( - driver_style_dc, - acc_curve, - dec_curve, - veh_max_speed, - car_info, - res_path - ) + dt = 0.1 # The simulation step in seconds + t = np.arange(0, duration + dt, dt) # sample time series + x, v, a, v_des, gr_idx, trq_gr_in, w_gr_in, p_gr_in = \ + np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)).astype(int), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)) + soc_ref, soc, fc, trq_ice, p_ice, p_fuel, trq_em, p_em, p_batt, i_batt, ef = \ + np.ones(len(t))*0.3, np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)) + if hyd_mode == 'CD': + soc0 = 0.8 + else: + soc0 = 0.3 + dw_gr_in = np.zeros(len(t)) + + gs_cnt = 10 + for i in range(len(t)): + if i == 0: + v[i] = v0 + soc[i] = soc0 + v_des[i] = f_des(x[i]) + gr_idx[i] = int(gs_th(v[i])) + w_gr_in[i] = v[i] / (car.tire_radius * (1 - car.driveline_slippage)) * (car.final_drive * car.gr[gr_idx[i]]) # rad/s + else: + gs_cnt += 1 + if gs_cnt < 10: + gr_idx[i] = gr_idx[i-1] + else: + if int(gs_th(v[i-1]))>gr_idx[i-1]: + gr_idx[i] = gr_idx[i-1]+1 + gs_cnt = 0 + elif int(gs_th(v[i-1]))0: + a[i] = 0 + v[i] = max(v[i-1] + a[i] * dt, 0) + a[i] = (v[i] - v[i-1]) / dt + x[i] = x[i-1] + (v[i] + v[i-1]) / 2 * dt + v_des[i] = f_des(x[i]) + + a_r = (car.f0 + car.f1*v[i]*3.6 + car.f2*pow(v[i]*3.6, 2)) / car.total_mass + a_t = a[i] + a_r + trq_gr_in[i] = a_t*car.total_mass*car.tire_radius / car.driveline_efficiency / (car.final_drive * car.gr[gr_idx[i]]) # Nm, Debug=50 + # https://www.engineeringtoolbox.com/angular-velocity-acceleration-power-torque-d_1397.html + # https://www.convertunits.com/from/RPM/to/rad/sec + w_gr_in[i] = v[i] / (car.tire_radius * (1 - car.driveline_slippage)) * (car.final_drive * car.gr[gr_idx[i]]) # rad/s, Debug=315 + p_gr_in[i] = trq_gr_in[i] * w_gr_in[i] + # dw_gr_in[i] = a[i] / (car.tire_radius * (1 - car.driveline_slippage)) * (car.final_drive * car.gr[gr_idx[i]]) + dw_gr_in[i] = a_t / (car.tire_radius * (1 - car.driveline_slippage)) * (car.final_drive * car.gr[gr_idx[i]]) + + if car.powertrain in ['hybrid', 'plug-in hybrid']: + if hyd_mode == 'CS': + soc[i], fc[i], trq_ice[i], p_ice[i], p_fuel[i], trq_em[i], p_em[i], p_batt[i], i_batt[i], ef[i] = \ + msim.powertrain_hyd_cs_aecms( + car, + soc[i-1], + soc_ref[i-1], + w_gr_in[i], + trq_gr_in[i], + dw_gr_in[i], + ) + if hyd_mode == 'CD': + soc[i], trq_em[i], p_em[i], p_batt[i], i_batt[i] = \ + msim.powertrain_hyd_cd( + car, + soc[i-1], + w_gr_in[i], + trq_gr_in[i], + ) + + + trq_gr_in[0] = trq_gr_in[1] + p_gr_in[0] = trq_gr_in[0]*w_gr_in[0] + a[0] = a[1] + + data1_plot = [ + t, v_des, x, v, a, gr_idx, w_gr_in*60/(2*np.pi), trq_gr_in, p_gr_in*1e-3, dw_gr_in] + data1_label = [ + 'time(s)', 'vn(m/s)', 'x(m)', 'v(m/s)', 'a(m/s2)', 'gr_idx', 'w_gr_in\n(rpm)', 'trq_gr_in\n(Nm)', 'p_gr_in\n(kW)', 'dw_gr_in'] + fig, axs = plt.subplots(len(data1_plot), sharex=True, figsize=(11,6)) + for i in range(len(data1_plot)): + axs[i].plot(t, data1_plot[i]) + if data1_label[i]=='v(m/s)': + axs[i].plot(t, v_des, 'r--') + if data1_label[i] in ['time(s)', 'vn(m/s)']: + continue + axs[i].set_ylabel(data1_label[i], rotation=0, labelpad=30) + axs[i].grid() + axs[i].set_xlabel('time(s)') + axs[i].set_xlim(0, t[-1]) + plt.savefig(os.path.join(res_path, 'mfc_driving_cycle_'+cycle_des+'_vehdyn.png')) + # plt.show() + + if car.powertrain in ['hybrid', 'plug-in hybrid']: + if hyd_mode == 'CS': + data2_plot = [ + soc, fc*1e3, trq_ice, p_ice*1e-3, trq_em, p_em*1e-3, p_batt*1e-3, i_batt, ef,] + data2_label = [ + 'soc', 'fc(ml)', 'trq_ice(Nm)', 'p_ice(kW)', 'trq_em(Nm)', 'p_em(kW)', 'p_batt(kW)', 'i_batt(A)', 'ef',] + if hyd_mode == 'CD': + data2_plot = [soc, trq_em, p_em*1e-3, p_batt*1e-3, i_batt,] + data2_label = ['soc', 'trq_em(Nm)', 'p_em(kW)', 'p_batt(kW)', 'i_batt(A)',] + fig, axs = plt.subplots(len(data2_plot), sharex=True, figsize=(11,6)) + for i in range(len(data2_plot)): + axs[i].plot(t, data2_plot[i]) + axs[i].set_ylabel(data2_label[i], rotation=0, labelpad=30) + axs[i].grid() + if data2_label[i]=='fc(ml)': + axs[i].text(200, 0.2, 'avg. fc (l/100km): '+str(round(np.sum(fc)/(x[-1]*1e-5), 1))) + axs[i].set_xlabel('time(s)') + axs[i].set_xlim(0, t[-1]) + plt.savefig(os.path.join(res_path, 'mfc_driving_cycle_'+cycle_des+'_powdyn.png')) + plt.show() + + df = pd.DataFrame.from_dict(dict(zip(data1_label+data2_label, data1_plot+data2_plot))) + df.to_csv(os.path.join(res_path, \ + 'ds'+str(round(driver_style,1)).replace('.', '')+'_gs'+str(round(gs_style,1)).replace('.', '')+'_'+hyd_mode+'_'+cycle_des+'.csv'), index=False) if __name__ == "__main__": @@ -81,6 +211,7 @@ def mfc_main( # 7565 - Golf, https://www.cars-data.com/en/volkswagen-golf-2.0-tdi-150hp-highline-specs/59579 # 26687 - Ioniq, https://www.cars-data.com/en/hyundai-ioniq-electric-comfort-specs/76078 # 26712 - Niro, https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-executiveline-specs/76106 + # 55559 - Golf 8, https://www.cars-data.com/en/volkswagen-golf-1-4-ehybrid-204hp-style-specs/166618/tech, [f0, f1, f2] = [115.5, 0.106, 0.03217] cars = [ # 0 - 'ICEV' @@ -88,22 +219,25 @@ def mfc_main( # 1 - 'EV' [26687], # 2 - 'HEV' - [26712], + [26712, 55559], ] - car_id = cars[2][0] # SELECT THE CAR - hyd_mode = ['Na', 'CD', 'CS'][1] # CD/CS, ONLY VALID FOR HEV, Na = not applicable + car_id = cars[2][1] ### Changable, SELECT THE CAR + if car_id in cars[2]: + hyd_mode = ['CD', 'CS'][1] ### Changable, CD/CS, ONLY VALID FOR HEV, Na = not applicable + else: + hyd_mode = 'Na' - driver_style = [1.0, 0.8, 0.6] - driver_style_dc = [0, 1, 2] + driver_style = [1.0, 0.8, 0.6][2] ### Changable + gs_style = [1.0, 0.8, 0.6][2] ### Changable veh_load = 75 * 0 db = rno.load_db_to_dictionary(db_name) car = rno.get_vehicle_from_db(db, car_id) - if hyd_mode == 'Na': - res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, 'curves') - else: - res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, hyd_mode, 'curves') + + res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, 'ds'+str(round(driver_style,1)).replace('.', '')+'_gs'+str(round(gs_style,1)).replace('.', '')) + if hyd_mode != 'Na': + res_path = os.path.join(res_path, hyd_mode) if not os.path.exists(res_path): os.makedirs(res_path, exist_ok=True) @@ -113,6 +247,6 @@ def mfc_main( hyd_mode, veh_load, driver_style, - driver_style_dc, + gs_style, res_path ) diff --git a/MAIN_hybridMFC/MAIN_hybrid_mfc_100_accel_time.py b/MAIN_hybridMFC/MAIN_hybrid_mfc_100_accel_time.py index 8b92599..98d1374 100644 --- a/MAIN_hybridMFC/MAIN_hybrid_mfc_100_accel_time.py +++ b/MAIN_hybridMFC/MAIN_hybrid_mfc_100_accel_time.py @@ -39,22 +39,24 @@ def process_0_100_accelerations(selected_car, Models): rt = 0.1 will_acc_model = 'horizontal_b' # gipps, idm, horizontal driver_style = 1 + gs_style = 1 overshoot = 0 # m/s NOT valid for horizontal wTa line over_f = 1.2 # surpassing (%) of the estimated acceleration curve of the vehicle freq = 10 # frequency - try: + if True: mfc_acc_curve = 0 mfc_dec_curve = 0 for model_name in Models: if model_name == 'MFC': veh_load = 75 * 0 - _ , mfc_acc_curve, mfc_dec_curve, _, _, _, _ \ + _, mfc_acc_curve, mfc_dec_curve, _, _, _, _, _, gs_th \ = msim.mfc_curves( my_car, carid, hyd_mode, - veh_load + veh_load, + gs_style, ) sstart = 0 # start speed @@ -65,6 +67,8 @@ def process_0_100_accelerations(selected_car, Models): parameters = { 'driver_style': driver_style, ### Driver style for MFC [0,1] + 'gs_style': gs_style, + 'gs_th': gs_th, 'mfc_acc_curve': mfc_acc_curve, ### Acceleration curve MFC 'mfc_dec_curve': mfc_dec_curve, ### Deceleration curve MFC 'will_acc_model': will_acc_model, ### Will to accelerate @@ -97,7 +101,7 @@ def process_0_100_accelerations(selected_car, Models): results_list_item.append(cnt_car_ok) results_list_item.append(cnt_car_over) - except: + else: error_car_list_item = carid print("Not able to compute %s curve for carid: %d" % (model_name, carid)) return results_list_item, error_car_list_item diff --git a/MAIN_hybridMFC/MAIN_hybrid_mfc_cal_val_Golf.py b/MAIN_hybridMFC/MAIN_hybrid_mfc_cal_val_Golf.py new file mode 100644 index 0000000..f5e4d24 --- /dev/null +++ b/MAIN_hybridMFC/MAIN_hybrid_mfc_cal_val_Golf.py @@ -0,0 +1,684 @@ +# %% +# Packages & paths +import sys, os, time +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +from scipy import interpolate, signal +import collections +import multiprocessing +from lmfit import Parameters, minimize +from sklearn.metrics import mean_squared_error +from sklearn.metrics.pairwise import cosine_similarity +from itertools import product +from functools import partial +from IPython.core import display as ICD + +pathRoot = os.path.abspath(__file__ + "/../../") +sys.path.append(pathRoot) + +import c2art_env.sim_env.hybrid_mfc.reading_n_organizing as rno +import c2art_env.sim_env.hybrid_mfc.microsim as msim +import c2art_env.utils as utils + + +# %% Define functions +def data_preprocess(data_path, Dataset): + df1 = pd.read_excel(os.path.join(data_path, Dataset)) + cols_ = df1.columns + + t = np.round(np.arange(round(df1['Time'].iloc[0]*10), round(df1['Time'].iloc[-1]*10+1), 1)*0.1, 1) + df2 = pd.DataFrame() + df2['Time [s]'] = t + for i in range(1, len(df1.columns)): + if cols_[i] == 'Engaged gear [-]': + f = interpolate.interp1d(df1['Time'], df1[cols_[i]], kind='nearest', fill_value='extrapolate') + df2[cols_[i]] = f(t) + else: + f = interpolate.interp1d(df1['Time'], df1[cols_[i]], fill_value='extrapolate') + df2[cols_[i]] = f(t) + + df2['Speed [m/s]'] = df2['Vehicle speed [km/h]'] / 3.6 + df2['Accel [m/s2]'] = np.append((utils.numbadiff(df2['Speed [m/s]'].to_numpy()) / utils.numbadiff(df2['Time [s]'].to_numpy())), 0) + + return df2 + + +def plt_driving_traj(df, fig_path): + f, axs = plt.subplots(len(df.columns)-1, 1, figsize=(10,8), sharex=True) + for i, col in enumerate(df.columns[1:]): + axs[i].plot(df['Time [s]'], df[col]) + axs[i].set_ylabel(col) + axs[i].grid() + plt.tight_layout() + plt.savefig(fig_path) + # plt.show() + + +def cal_val_traj(df1, df2, car, hyd_mode, res_path): + cal_res_path = os.path.join(res_path, 'calibration') + val_res_path = os.path.join(res_path, 'validation') + for xpath in [cal_res_path, val_res_path]: + if not os.path.exists(xpath): + os.makedirs(xpath, exist_ok=True) + + Models = collections.OrderedDict() + Models['Gipps'] = msim.follow_leader_gipps + Models['IDM'] = msim.follow_leader_idm + Models['MFC'] = msim.follow_leader_mfc + + Models_results = collections.OrderedDict() + for model_name in Models: + Models_results[model_name] = {} + + results_list = [] + for lap_num, df in enumerate([df1, df2]): + testDataset = hyd_mode + '_' + str(lap_num+1) + + ltp = np.array(df['Time [s]']-df['Time [s]'].iloc[0]) + lsp = np.array(df['Speed [m/s]']) + lap = np.array(df['Accel [m/s2]']) + + ltp_long = np.arange(0, round(2 * len(lsp) / 10, 1), 0.1) + freq = 10 + + # This test is about car following + start_dist = 10000 + start_speed = lsp[0] + bn = -3 + rt = 1 + beta = 0.025 + gamma = 0.5 + delta = 4 + + # ldp = [i * 0.1 + 0.005 * j for i, j in zip(lsp, lap)] + ldp = [0] + [(lsp[i]+lsp[i-1]) / 2 * 0.1 for i in range(1, len(lsp))] + ldt = np.cumsum(ldp) + dist_travelled = ldt[-1] + interp_gt = interpolate.interp1d(ldt, lsp) + + # Creating a distance traveled with equally spaced points - 2 meters distance + gt_dist = np.arange(50, int(round(dist_travelled - 50)), 2) + lsp_dist = interp_gt(gt_dist) + + # Compute acceleration based on the speed diff and the distance traveled + lap_dist, ltp_dist = [], [0] + for i in range(1, len(lsp_dist)): + lap_dist.append((lsp_dist[i] - lsp_dist[i - 1]) * (lsp_dist[i - 1] + lsp_dist[i]) / 4) + ltp_dist.append(4 / (lsp_dist[i - 1] + lsp_dist[i])) + lap_dist.append(0) + lap_dist = np.array(lap_dist) + ltp_dist = np.cumsum(np.array(ltp_dist)) + ldt_dist = gt_dist + + # Interpolation to get the distance cycle at each point + df_ = pd.DataFrame.from_dict( + {'ldt': ldt[1:], + 'lsp': lsp[:-1]} + ) + df_.drop_duplicates(subset=['ldt'], inplace=True) + cycle_dist_cubic = interpolate.CubicSpline(df_['ldt'], df_['lsp']) + + # Dictionary for the lmfit optimization + min_lmfit = {} + tmpResDict = collections.OrderedDict() + tmpResDict['GroundTruth'] = {} + for model_name in Models: + model = Models[model_name] + instance = { + 'vn': 0, ### Desired speed of the follower in m/s + 'bn': bn, ### Maximum deceleration of the follower in m/s2 + 'an': np.random.uniform(0, 8), ### Maximum acceleration of the follower in m/s2 + 's0': 1, ### Minimum distance in meters + 'rt': rt, ### Reaction time in sec + 'car_id': car_id, + 'car': car, ### The car for the MFC + 'hyd_mode': hyd_mode, + 'veh_load': 75 * 0, + 'will_acc_model': 'horizontal_b', ### Will to accelerate + 'overshoot': 0, ### overshoot - not used + 'automatic': car.transmission, ### Gear box + 'mfc_curve': False, + 'gs': False, + 'driver_style': np.random.uniform(0, 1), + 'gs_style': np.random.uniform(0, 1), + 'followDistance': True, + 'distance_cycle': cycle_dist_cubic, + 'distTravelLimit': dist_travelled, + 'interp_gt': interp_gt, + } + + ficticious_leader = [30] * len(ltp_long) + + # Start of the lmfit optimization + params_lmfit = Parameters() + if model == msim.follow_leader_gipps: + # params_lmfit.add('an', value=1, vary=True, min=0.5, max=4.0) + params_lmfit.add('an', value=1, vary=True, min=0.5, max=5.0) + params_lmfit.add('bn', value=bn, vary=False, min=-4.0, max=-1) + params_lmfit.add('rt', value=rt, vary=False, min=1, max=20) + params_lmfit.add('beta', value=beta, vary=True, min=0.001, max=5.0) + params_lmfit.add('gamma', value=gamma, vary=True, min=0.1, max=2.0) + if model == msim.follow_leader_idm: + # params_lmfit.add('an', value=1, vary=True, min=0.5, max=4.0) + params_lmfit.add('an', value=1, vary=True, min=0.5, max=5.0) + params_lmfit.add('delta', value=delta, vary=True, min=0.1, max=10.0) + params_lmfit.add('bn', value=bn, vary=False, min=-4.0, max=-1) + if model == msim.follow_leader_mfc: + # We need first to run Gipps to calibrate an, bn and rt + params_lmfit.add('driver_style', value=1, vary=True, min=0.1, max=1) + params_lmfit.add('gs_style', value=1, vary=True, min=0.1, max=1) + params_lmfit.add('bn', value=bn, vary=False, min=-4.0, max=-1) + params_lmfit.add('rt', value=rt, vary=False, min=1, max=20) + + min_lmfit[model_name] = minimize(msim.fcn2minGA_freeflow, params_lmfit, + args=( + instance, model, ltp_long, ficticious_leader, start_dist, start_speed), + method='Powell', tol=10e-5) + + lsp_dist_cycle = cycle_dist_cubic(gt_dist) + # Plot the fitted models - LMFit + fig1 = plt.figure(figsize=(8, 6)) + plt.plot(ldt_dist, lsp_dist, label='ref. vehicle') + plt.plot(ldt_dist, lsp_dist_cycle, label='cycle') + plt.title('Distance - Speed (part of a lap)', fontsize=18) + df = pd.DataFrame() + df['ldt_dist'] = ldt_dist + df['lsp_dist'] = lsp_dist + df['lsp_dist_cycle'] = lsp_dist_cycle + df['ltp_dist'] = ltp_dist + + for model_name in Models: + mfit = min_lmfit[model_name] + model = Models[model_name] + Params = collections.OrderedDict() + if model == msim.follow_leader_gipps: + Params = { + 'an': mfit.params['an'].value, + 'rt': mfit.params['rt'].value, + 'bn': mfit.params['bn'].value, + 'beta': mfit.params['beta'].value, + 'gamma': mfit.params['gamma'].value, + } + if model == msim.follow_leader_idm: + Params = { + 'an': mfit.params['an'].value, + 'bn': mfit.params['bn'].value, + 'delta': mfit.params['delta'].value, + } + if model == msim.follow_leader_mfc: + Params = { + 'rt': mfit.params['rt'].value, + # 'bn': mfit.params['bn'].value, + 'driver_style': mfit.params['driver_style'].value, + 'gs_style': mfit.params['gs_style'].value, + } + + Params = collections.OrderedDict(sorted(Params.items())) + + tmpResDict[model_name] = Params + + new_instance = {**instance, **Params} + + sim_step = 0.1 + ficticious_leader = [30] * len(ltp_long) + m_sp = list(model(new_instance, ltp_long, ficticious_leader, sim_step, start_dist, start_speed)) + m_ap = np.append(np.diff(m_sp), 0) * 10 + # m_dt = list(np.cumsum([i * 0.1 + 0.005 * j for i, j in zip(m_sp, m_ap)])) + m_dt = list(np.cumsum([0] + [(m_sp[i]+m_sp[i-1]) / 2 * 0.1 for i in range(1, len(m_sp))])) + + interp_m = interpolate.interp1d(m_dt, m_sp, fill_value='extrapolate') + + m_sp_1 = np.asarray(m_sp) + m_dt_1 = np.asarray(m_dt) + + # Store interpolation + Models_results[model_name]['interp'] = interp_m + + m_sp_dist = interp_m(gt_dist) + # m_ap_dist = np.append(np.diff(m_sp_dist), 0) * 10 + m_ap_dist, m_tp_dist = [], [0] + for i in range(1, len(m_sp_dist)): + m_ap_dist.append((m_sp_dist[i] - m_sp_dist[i - 1]) * ((m_sp_dist[i - 1] + m_sp_dist[i]) / 4)) + m_tp_dist.append(4 / (m_sp_dist[i - 1] + m_sp_dist[i])) + m_ap_dist.append(0) + m_ap_dist = np.array(m_ap_dist) + m_tp_dist = np.cumsum(np.array(m_tp_dist)) + m_dt_dist = gt_dist + + rmse_sp = np.sqrt(mean_squared_error(lsp_dist, m_sp_dist)) + rmse_ap = np.sqrt(mean_squared_error(lap_dist, m_ap_dist)) + rmse_tp = np.sqrt(mean_squared_error(ltp_dist, m_tp_dist)) + + similarity_sp = cosine_similarity(lsp_dist.reshape(1,-1), m_sp_dist.reshape(1,-1)) + + # Save rmsn errors + tmpResDict[model_name]['file'] = testDataset + tmpResDict[model_name]['lap'] = (lap_num + 1) + tmpResDict[model_name]['model'] = model_name + tmpResDict[model_name]['rmse_sp'] = rmse_sp + tmpResDict[model_name]['std_sp'] = np.std(np.array(lsp_dist) - np.array(m_sp_dist)) + tmpResDict[model_name]['rmse_ap'] = rmse_ap + tmpResDict[model_name]['std_ap'] = np.std(np.array(lap_dist) - np.array(m_ap_dist)) + tmpResDict[model_name]['rmse_tp'] = rmse_tp + tmpResDict[model_name]['std_tp'] = np.std(np.array(ltp_dist) - np.array(m_tp_dist)) + tmpResDict[model_name]['fit'] = mfit.residual[0] + + print(mfit.params.valuesdict()) + print(mfit.residual[0]) + print('rmse_speed: %.3f' % rmse_sp) + print('std_sp: %.3f' % np.std(np.array(lsp_dist) - np.array(m_sp_dist))) + print('rmse_acc: %.3f' % rmse_ap) + print('std_ap: %.3f' % np.std(np.array(lap_dist) - np.array(m_ap_dist))) + print('rmse_dist: %.3f' % rmse_tp) + print('std_tp: %.3f' % np.std(np.array(ltp_dist) - np.array(m_tp_dist))) + + Models_results[model_name]['sp'] = list(m_sp) + Models_results[model_name]['sp_dist'] = list(m_sp_dist) + Models_results[model_name]['ap_dist'] = list(m_ap_dist) + Models_results[model_name]['dt_dist'] = list(m_dt_dist) + Models_results[model_name]['tp_dist'] = list(m_tp_dist) + + df[model_name+'_sp_dist'] = Models_results[model_name]['sp_dist'] + df[model_name+'_ap_dist'] = Models_results[model_name]['ap_dist'] + df[model_name+'_dt_dist'] = Models_results[model_name]['dt_dist'] + df[model_name+'_tp_dist'] = Models_results[model_name]['tp_dist'] + + if model_name == 'Gipps': + linest = '-.' + elif model_name == 'IDM': + linest = ':' + else: + linest = '--' + + plt.plot(m_dt_dist, m_sp_dist, label=model_name, linestyle=linest) + plt.legend(loc='best') + plt.ylabel('Speed $(m/s)$', fontsize=16) + plt.xlabel('Distance $(m)$', fontsize=16) + + plt.savefig(os.path.join(cal_res_path, '%s_models_distance_speed_part.png' % testDataset), dpi=150) + plt.close(fig1) + df.to_csv(os.path.join(cal_res_path, '%s_models_part.csv' % testDataset), index=False) + + # Macro stats + print('Average speed GT:%.2f' % np.mean(lsp_dist)) + + print('Average absolute acceleration GT:%.2f' % (np.mean(np.absolute(lap_dist)))) + + # Save macro results + tmpResDict['GroundTruth']['avg_speed'] = np.mean(lsp_dist) + tmpResDict['GroundTruth']['avg_absolute_acceleration'] = np.mean(np.absolute(lap_dist)) + tmpResDict['GroundTruth']['duration'] = len(lsp) / freq + tmpResDict['GroundTruth']['file'] = testDataset + tmpResDict['GroundTruth']['model'] = 'GroundTruth' + + for model_name in Models: + m_sp_dist = Models_results[model_name]['sp_dist'] + print('Average speed %s:%.2f' % (model_name, np.mean(m_sp_dist))) + m_ap_dist = Models_results[model_name]['ap_dist'] + print('Average absolute acceleration %s:%.2f' % (model_name, np.mean(np.absolute(m_ap_dist)))) + m_sp = Models_results[model_name]['sp'] + print('Time dev %s:%d sec' % (model_name, (len(m_sp) - len(lsp)) / freq)) + + # Save macro results + tmpResDict[model_name]['avg_speed'] = np.mean(m_sp_dist) + tmpResDict[model_name]['avg_absolute_acceleration'] = np.mean(np.absolute(m_ap_dist)) + tmpResDict[model_name]['time_difference'] = (len(m_sp) - len(lsp)) / freq + + # Append the results for the specific file to the total results' list. + results_list.append(tmpResDict['GroundTruth']) + for model_name in Models: + results_list.append(tmpResDict[model_name]) + + # Plot of cycle - Acceleration + fig1, ax = plt.subplots(figsize=(8, 6)) + plt.plot(ldt_dist, lap_dist, label='ref. vehicle') + + # plt.plot(ldt, cycle_sp, label='cycle') + for model_name in Models: + m_dt_dist = Models_results[model_name]['dt_dist'] + m_ap_dist = Models_results[model_name]['ap_dist'] + if model_name == 'Gipps': + linest = '-.' + elif model_name == 'IDM': + linest = ':' + else: + linest = '--' + plt.plot(m_dt_dist, m_ap_dist, label=model_name, linestyle=linest) + + plt.ylabel('Acceleration $(m/s^2)$', fontsize=16) + plt.xlabel('Distance $(m)$', fontsize=16) + plt.title('Distance - Acceleration (part of a lap)', fontsize=18) + plt.legend(loc='best') + + plt.savefig(os.path.join(cal_res_path, '%s_models_distance_acceleration_part.png' % testDataset), dpi=150) + + plt.close(fig1) + + #################################################### + # Plot of cycle - Speed over Time + fig1, ax = plt.subplots() + plt.plot(ltp_dist, lsp_dist, label='ref. vehicle') + for model_name in Models: + m_tp_dist = Models_results[model_name]['tp_dist'] + m_sp_dist = Models_results[model_name]['sp_dist'] + plt.plot(m_tp_dist, m_sp_dist, label=model_name) + + plt.ylabel('Speed $(m/s)$') + plt.xlabel('Time $(s)$') + plt.title('Time - Speed (1 lap)') + plt.legend(loc='best') + # plt.show() + plt.savefig(os.path.join(cal_res_path, '%s_models_time_speed.png' % testDataset), dpi=300) + plt.close(fig1) + + # Plot of cycle - Acceleration over Time + fig1, ax = plt.subplots() + plt.plot(ltp_dist, lap_dist, label='ref. vehicle') + for model_name in Models: + m_tp_dist = Models_results[model_name]['tp_dist'] + m_ap_dist = Models_results[model_name]['ap_dist'] + plt.plot(m_tp_dist, m_ap_dist, label=model_name) + + plt.ylabel('Acceleration $(m/s^2)$') + plt.xlabel('Time $(s)$') + plt.title('Time - Acceleration (1 lap)') + plt.legend(loc='best') + # plt.show() + plt.savefig(os.path.join(cal_res_path, '%s_models_time_acceleration.png' % testDataset), dpi=300) + plt.close(fig1) + + if not results_list: + print('empty list') + else: + cal_res_df = pd.DataFrame(results_list) + cal_res_df.to_csv(os.path.join(cal_res_path, 'free_flow_calibration.csv'), + index=False) + + # Validation + results_list = [] + for lap_num, df in enumerate([df1, df2]): + testDataset = hyd_mode + '_' + str(lap_num+1) + if lap_num+1 == 1: + CalibrateDataset = hyd_mode + '_2' + elif lap_num+1 == 2: + CalibrateDataset = hyd_mode + '_1' + + ltp = np.array(df['Time [s]']-df['Time [s]'].iloc[0]) + lsp = np.array(df['Speed [m/s]']) + lap = np.array(df['Accel [m/s2]']) + + ltp_long = np.arange(0, round(2 * len(lsp) / 10, 1), 0.1) + freq = 10 + + # This test is about car following + start_dist = 10000 + start_speed = lsp[0] + bn = -3 + rt = 1 + beta = 0.025 + gamma = 0.5 + delta = 4 + + # ldp = [i * 0.1 + 0.005 * j for i, j in zip(lsp, lap)] + ldp = [0] + [(lsp[i]+lsp[i-1]) / 2 * 0.1 for i in range(1, len(lsp))] + ldt = np.cumsum(ldp) + dist_travelled = ldt[-1] + interp_gt = interpolate.interp1d(ldt, lsp) + + # Creating a distance traveled with equally spaced points - 2 meters distance + gt_dist = np.arange(50, int(round(dist_travelled - 50)), 2) + lsp_dist = interp_gt(gt_dist) + + # Compute acceleration based on the speed diff and the distance traveled + lap_dist, ltp_dist = [], [0] + for i in range(1, len(lsp_dist)): + lap_dist.append((lsp_dist[i] - lsp_dist[i - 1]) * (lsp_dist[i - 1] + lsp_dist[i]) / 4) + ltp_dist.append(4 / (lsp_dist[i - 1] + lsp_dist[i])) + lap_dist.append(0) + lap_dist = np.array(lap_dist) + ltp_dist = np.cumsum(np.array(ltp_dist)) + ldt_dist = gt_dist + + # Interpolation to get the distance cycle at each point + cycle_dist_cubic = interpolate.CubicSpline(ldt[1:], lsp[:-1]) + + # Save results + tmpResDict = collections.OrderedDict() + tmpResDict['GroundTruth'] = {} + + lsp_dist_cycle = cycle_dist_cubic(gt_dist) + fig_dist_speed = plt.figure() + ax_dist_speed = fig_dist_speed.add_subplot(311) + ax_dist_speed.plot(ldt_dist, lsp_dist, label='ref. vehicle') + ax_dist_speed.plot(ldt_dist, lsp_dist_cycle, label='cycle') + ax_dist_speed.set_title('Distance - Speed (1 lap)') + ax_dist_speed.set_ylabel('Speed $(m/s)$') + ax_dist_speed.set_xlabel('Distance $(m)$') + + ax_dist_acc = fig_dist_speed.add_subplot(312) + ax_dist_acc.plot(ldt_dist, lap_dist, label='ref. vehicle') + ax_dist_acc.set_title('Distance - Acceleration (1 lap)') + ax_dist_acc.set_ylabel('Acceleration $(m/s^2)$') + ax_dist_acc.set_xlabel('Distance $(m)$') + + ax_time_speed = fig_dist_speed.add_subplot(313) + ax_time_speed.plot(ltp, lsp, label='ref. vehicle') + ax_time_speed.set_title('Time - Speed (1 lap)') + ax_time_speed.set_ylabel('Speed $(m/s)$') + ax_time_speed.set_xlabel('Time $(s)$') + + df = pd.DataFrame() + df['ldt_dist'] = ldt_dist + df['lsp_dist'] = lsp_dist + df['lsp_dist_cycle'] = lsp_dist_cycle + df['lap_dist'] = lap_dist + # df['ltp'] = ltp + # df['lsp'] = lsp + + for model_name in Models: + model = Models[model_name] + instance = { + 'vn': 0, ### Desired speed of the follower in m/s + 'bn': bn, ### Maximum deceleration of the follower in m/s2 + 'an': np.random.uniform(0, 8), ### Maximum acceleration of the follower in m/s2 + 's0': 1, ### Minimum distance in meters + 'rt': rt, ### Reaction time in sec + 'car_id': car_id, + 'car': car, ### The car for the MFC + 'hyd_mode': hyd_mode, + 'veh_load': 75 * 0, + 'will_acc_model': 'horizontal_b', ### Will to accelerate + 'overshoot': 0, ### overshoot - not used + 'automatic': car.transmission, ### Gear box + 'mfc_curve': False, + 'gs': False, + 'driver_style': np.random.uniform(0, 1), + 'gs_style': np.random.uniform(0, 1), + 'followDistance': True, + 'distance_cycle': cycle_dist_cubic, + 'distTravelLimit': dist_travelled, + 'interp_gt': interp_gt, + } + + ficticious_leader = [30] * len(ltp_long) + Params = collections.OrderedDict() + if model == msim.follow_leader_gipps: + Params = { + 'an': cal_res_df['an'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'rt': cal_res_df['rt'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'bn': cal_res_df['bn'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'beta': cal_res_df['beta'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'gamma': cal_res_df['gamma'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + } + if model == msim.follow_leader_idm: + Params = { + 'an': cal_res_df['an'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'bn': cal_res_df['bn'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'delta': cal_res_df['delta'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + } + if model == msim.follow_leader_mfc: + # We need first to run Gipps to calibrate an, bn and rt + Params = { + 'rt': cal_res_df['rt'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'driver_style': cal_res_df['driver_style'].loc[ + (cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'gs_style': cal_res_df['gs_style'].loc[ + (cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + } + + Params = collections.OrderedDict(sorted(Params.items())) + + # Save the parameters used for validation + tmpResDict[model_name] = Params + + new_instance = {**instance, **Params} + + sim_step = 0.1 + m_sp = list(model(new_instance, ltp_long, ficticious_leader, sim_step, start_dist, start_speed)) + m_ap = np.append(np.diff(m_sp), 0) * 10 + # m_dt = list(np.cumsum([i * 0.1 + 0.005 * j for i, j in zip(m_sp, m_ap)])) + m_dt = list(np.cumsum([0] + [(m_sp[i]+m_sp[i-1]) / 2 * 0.1 for i in range(1, len(m_sp))])) + + interp_m = interpolate.interp1d(m_dt, m_sp) + + m_sp_1 = np.asarray(m_sp) + m_dt_1 = np.asarray(m_dt) + + m_sp_dist = interp_m(gt_dist) + m_ap_dist = np.append(np.diff(m_sp_dist), 0) * 10 + m_ap_dist, m_tp_dist = [], [0] + for i in range(1, len(m_sp_dist)): + m_ap_dist.append((m_sp_dist[i] - m_sp_dist[i - 1]) * ((m_sp_dist[i - 1] + m_sp_dist[i]) / 4)) + m_tp_dist.append(4 / (m_sp_dist[i - 1] + m_sp_dist[i])) + m_ap_dist.append(0) + m_ap_dist = np.array(m_ap_dist) + m_tp_dist = np.cumsum(np.array(m_tp_dist)) + m_dt_dist = gt_dist + + rmse_sp = np.sqrt(mean_squared_error(lsp_dist, m_sp_dist)) + rmse_ap = np.sqrt(mean_squared_error(lap_dist, m_ap_dist)) + rmse_tp = np.sqrt(mean_squared_error(ltp_dist, m_tp_dist)) + + rel_speed = [k / j for k, j in zip(m_sp_dist, lsp_dist)] + opt_metric = np.sum([np.log(k) ** 2 for k in rel_speed]) + + # Save rmsn errors + tmpResDict[model_name]['file'] = testDataset + tmpResDict[model_name]['model'] = model_name + tmpResDict[model_name]['rmse_sp'] = rmse_sp + tmpResDict[model_name]['std_sp'] = np.std(np.array(lsp_dist) - np.array(m_sp_dist)) + tmpResDict[model_name]['rmse_ap'] = rmse_ap + tmpResDict[model_name]['std_ap'] = np.std(np.array(lap_dist) - np.array(m_ap_dist)) + tmpResDict[model_name]['rmse_tp'] = rmse_tp + tmpResDict[model_name]['std_tp'] = np.std(np.array(ltp_dist) - np.array(m_tp_dist)) + tmpResDict[model_name]['fit'] = opt_metric + + print('Average speed %s:%.2f' % (model_name, np.mean(m_sp_dist))) + print('Average absolute acceleration %s:%.2f' % (model_name, np.mean(np.absolute(m_ap_dist)))) + print('Time dev %s:%.2f sec' % (model_name, (len(m_sp) - len(lsp)) / freq)) + + # Save macro results + tmpResDict[model_name]['avg_speed'] = np.mean(m_sp_dist) + tmpResDict[model_name]['avg_absolute_acceleration'] = np.mean(np.absolute(m_ap_dist)) + tmpResDict[model_name]['time_difference'] = (len(m_sp) - len(lsp)) / freq + + print('fit: %.3d' % opt_metric) + print('rmse_speed: %.3f' % rmse_sp) + print('rmse_acc: %.3f' % rmse_ap) + print('rmse_dist: %.3f' % rmse_tp) + ax_dist_speed.plot(m_dt_dist, m_sp_dist, label='%s RMSE(time):%.3f'%(model_name, rmse_tp)) + ax_dist_acc.plot(m_dt_dist, m_ap_dist, label='%s RMSE(time):%.3f'%(model_name, rmse_tp)) + ax_time_speed.plot(m_tp_dist, m_sp_dist, label='%s RMSE(time):%.3f'%(model_name, rmse_tp)) + + df[model_name+'_m_dt_dist'] = m_dt_dist + df[model_name+'_m_sp_dist'] = m_sp_dist + df[model_name+'_m_ap_dist'] = m_ap_dist + df[model_name+'_m_tp_dist'] = m_tp_dist + + fig_dist_speed.subplots_adjust(left=0.10, bottom=0.10, right=0.90, top=0.95, hspace=0.20) + ax_dist_speed.legend(bbox_to_anchor=(1.1, 1.05)) + ax_dist_acc.legend(bbox_to_anchor=(1.1, 1.05)) + ax_time_speed.legend(bbox_to_anchor=(1.1, 1.05)) + + plt.tight_layout() + # plt.show() + plt.savefig(os.path.join(val_res_path, '%s_models_distance_speed.png' % testDataset), dpi=300) + plt.close(fig_dist_speed) + + # Macro stats + print('Average speed GT:%.2f' % np.mean(lsp_dist)) + print('Average absolute acceleration GT:%.2f' % (np.mean(np.absolute(lap_dist)))) + + # Save macro results + tmpResDict['GroundTruth']['avg_speed'] = np.mean(lsp_dist) + tmpResDict['GroundTruth']['avg_absolute_acceleration'] = np.mean(np.absolute(lap_dist)) + tmpResDict['GroundTruth']['duration'] = len(lsp) / freq + tmpResDict['GroundTruth']['file'] = testDataset + tmpResDict['GroundTruth']['model'] = 'GroundTruth' + + # Append the results for the specific file to the total results' list. + results_list.append(tmpResDict['GroundTruth']) + for model_name in Models: + results_list.append(tmpResDict[model_name]) + + if not results_list: + print('empty list') + else: + val_res_df = pd.DataFrame(results_list) + val_res_df.to_csv(os.path.join(val_res_path, 'free_flow_validation.csv'), + index=False) + +# %% +if __name__ == "__main__": + car_id = 55559 # Golf 8, https://www.cars-data.com/en/volkswagen-golf-1-4-ehybrid-204hp-style-specs/166618/tech + db_name = os.path.join(pathRoot, 'c2art_env', 'sim_env', 'hybrid_mfc', + 'car_database', '2019_07_03_car_db_full') + db = rno.load_db_to_dictionary(db_name) + car = rno.get_vehicle_from_db(db, car_id) + + data_path = os.path.join(pathRoot, 'c2art_env', 'sim_env', 'hybrid_mfc', + 'datasets', 'volkswagen_golf8') + trajDataset = [ + 'logfile_raw2022-04-01_17-55-56__processed_1.xlsx', # CD - driver 2 + 'logfile_raw2022-04-01_17-55-56__processed_3.xlsx', # CD - driver 2 + 'logfile_raw2022-02-19_19-50-06__processed_2.xlsx', # CS - driver 9 + 'logfile_raw2022-02-19_19-50-06__processed_3.xlsx', # CS - driver 9 + ] + + # """ + hyd_mode = 'CD' # driver 2 + res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, hyd_mode, 'traj_cal_val') + if not os.path.exists(res_path): + os.makedirs(res_path, exist_ok=True) + df_cd_a = data_preprocess(data_path, trajDataset[0]) + df_cd_a = df_cd_a.loc[(df_cd_a['Time [s]']>=250.0) & (df_cd_a['Time [s]']<=430.0)].reset_index(drop=True) + df_cd_a['Time [s]'] = df_cd_a['Time [s]'] - df_cd_a['Time [s]'][0] + df_cd_a.to_csv(os.path.join(res_path, 'Interp_Smooth_'+hyd_mode+'_a'+'.csv'), index=False) + + df_cd_b = data_preprocess(data_path, trajDataset[1]) + # df_cd_b = df_cd_b.loc[df_['Time [s]']>=0.0].reset_index(drop=True) + df_cd_b['Time [s]'] = df_cd_b['Time [s]'] - df_cd_b['Time [s]'][0] + df_cd_b.to_csv(os.path.join(res_path, 'Interp_Smooth_'+hyd_mode+'_b'+'.csv'), index=False) + + cal_val_traj(df_cd_a, df_cd_b, car, hyd_mode, res_path) + # """ + + # """ + hyd_mode = 'CS' # driver 9 + res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, hyd_mode, 'traj_cal_val') + if not os.path.exists(res_path): + os.makedirs(res_path, exist_ok=True) + df_cs_a = data_preprocess(data_path, trajDataset[2]) + df_cs_a = df_cs_a.loc[(df_cs_a['Time [s]']>=1660.0) & (df_cs_a['Time [s]']<=3330.0)].reset_index(drop=True) + df_cs_a['Time [s]'] = df_cs_a['Time [s]'] - df_cs_a['Time [s]'][0] + df_cs_a.to_csv(os.path.join(res_path, 'Interp_Smooth_'+hyd_mode+'_a'+'.csv'), index=False) + + df_cs_b = data_preprocess(data_path, trajDataset[3]) + df_cs_b = df_cs_b.loc[(df_cs_b['Time [s]']>=3520.0) & (df_cs_b['Time [s]']<=6650.0)].reset_index(drop=True) + df_cs_b['Time [s]'] = df_cs_b['Time [s]'] - df_cs_b['Time [s]'][0] + df_cs_b.to_csv(os.path.join(res_path, 'Interp_Smooth_'+hyd_mode+'_b'+'.csv'), index=False) + + cal_val_traj(df_cs_a, df_cs_b, car, hyd_mode, res_path) + # """ +# %% diff --git a/MAIN_hybridMFC/MAIN_hybrid_mfc_cal_val_Niro.py b/MAIN_hybridMFC/MAIN_hybrid_mfc_cal_val_Niro.py new file mode 100644 index 0000000..0f5f7b5 --- /dev/null +++ b/MAIN_hybridMFC/MAIN_hybrid_mfc_cal_val_Niro.py @@ -0,0 +1,693 @@ +# %% +# Packages & paths +import sys, os, time +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +from scipy import interpolate, signal +import collections +import multiprocessing +from lmfit import Parameters, minimize +from sklearn.metrics import mean_squared_error +from sklearn.metrics.pairwise import cosine_similarity +from itertools import product +from functools import partial +from IPython.core import display as ICD + +pathRoot = os.path.abspath(__file__ + "/../../") +sys.path.append(pathRoot) + +import c2art_env.sim_env.hybrid_mfc.reading_n_organizing as rno +import c2art_env.sim_env.hybrid_mfc.microsim as msim +import c2art_env.utils as utils + + +# %% Define functions +def data_preprocess(data_path, Dataset, hyd_mode, res_path): + + df = pd.read_csv(os.path.join(data_path, Dataset), \ + usecols=['Time [s]', 'Current speed [km/h]', 'EngineRPM [rpm]', 'StateOfChargeBMS [%]', 'PedalPosition_1 %']) + df['Time [s]'] = df['Time [s]'] - df['Time [s]'][0] + df['Speed [m/s]'] = df['Current speed [km/h]'] / 3.6 + df['Accel [m/s2]'] = np.append((utils.numbadiff(df['Speed [m/s]'].to_numpy()) / utils.numbadiff(df['Time [s]'].to_numpy())), 0) + + t = np.round(np.arange(0, round(df['Time [s]'].iloc[-1]*10+1), 1)*0.1, 1) + dfraw, dfnew = pd.DataFrame(), pd.DataFrame() + dfraw['Time [s]'], dfnew['Time [s]'] = t, t + ls_col = ['Speed [m/s]', 'EngineRPM [rpm]', 'StateOfChargeBMS [%]', 'PedalPosition_1 %'] + for col in ls_col: + f = interpolate.interp1d(df['Time [s]'], df[col], fill_value='extrapolate') + yraw = f(t) + if col == 'Speed [m/s]': + yraw = np.maximum(yraw, 0) + ynew = signal.savgol_filter(yraw, 51, 3) + ynew = np.maximum(ynew, 0) + elif col == 'StateOfChargeBMS [%]': + ynew = signal.savgol_filter(yraw, 151, 1) + else: + ynew = yraw + dfraw[col] = yraw + dfnew[col] = ynew + dfraw['Accel [m/s2]'] = np.append((utils.numbadiff(dfraw['Speed [m/s]'].to_numpy()) / utils.numbadiff(dfraw['Time [s]'].to_numpy())), 0) + dfnew['Accel [m/s2]'] = np.append((utils.numbadiff(dfnew['Speed [m/s]'].to_numpy()) / utils.numbadiff(dfnew['Time [s]'].to_numpy())), 0) + + if hyd_mode == 'CD': + dfraw = dfraw.loc[dfraw['Time [s]']<=2040] + dfnew = dfnew.loc[dfnew['Time [s]']<=2040] + dfraw.to_csv(os.path.join(res_path, 'Interp_'+hyd_mode+'.csv'), index=False) + dfnew.to_csv(os.path.join(res_path, 'Interp_Smooth_'+hyd_mode+'.csv'), index=False) + + f, axs = plt.subplots(len(ls_col)+1, 1, figsize=(10,8), sharex=True) + for i, col in enumerate(ls_col + ['Accel [m/s2]']): + axs[i].plot(df['Time [s]'], df[col], label='Exp') + axs[i].plot(dfraw['Time [s]'], dfraw[col], 'r--', label='Interp') + axs[i].plot(dfnew['Time [s]'], dfnew[col], label='Interp+Smooth') + axs[i].set_ylabel(col) + axs[i].grid() + axs[i].legend() + plt.savefig(os.path.join(res_path, 'InterpSmooth_'+hyd_mode+'.png')) + # plt.show() + + return dfnew + + +def plt_driving_traj(df, fig_path): + f, axs = plt.subplots(len(df.columns)-1, 1, figsize=(10,8), sharex=True) + for i, col in enumerate(df.columns[1:]): + axs[i].plot(df['Time [s]'], df[col]) + axs[i].set_ylabel(col) + axs[i].grid() + plt.tight_layout() + plt.savefig(fig_path) + # plt.show() + + +def cal_val_traj(df1, df2, car, hyd_mode, res_path): + cal_res_path = os.path.join(res_path, 'calibration') + val_res_path = os.path.join(res_path, 'validation') + for xpath in [cal_res_path, val_res_path]: + if not os.path.exists(xpath): + os.makedirs(xpath, exist_ok=True) + + Models = collections.OrderedDict() + Models['Gipps'] = msim.follow_leader_gipps + Models['IDM'] = msim.follow_leader_idm + Models['MFC'] = msim.follow_leader_mfc + + Models_results = collections.OrderedDict() + for model_name in Models: + Models_results[model_name] = {} + + results_list = [] + for lap_num, df in enumerate([df1, df2]): + testDataset = hyd_mode + '_' + str(lap_num+1) + + ltp = np.array(df['Time [s]']-df['Time [s]'].iloc[0]) + lsp = np.array(df['Speed [m/s]']) + lap = np.array(df['Accel [m/s2]']) + + ltp_long = np.arange(0, round(2 * len(lsp) / 10, 1), 0.1) + freq = 10 + + # This test is about car following + start_dist = 10000 + start_speed = lsp[0] + bn = -3 + rt = 1 + beta = 0.025 + gamma = 0.5 + delta = 4 + + # ldp = [i * 0.1 + 0.005 * j for i, j in zip(lsp, lap)] + ldp = [0] + [(lsp[i]+lsp[i-1]) / 2 * 0.1 for i in range(1, len(lsp))] + ldt = np.cumsum(ldp) + dist_travelled = ldt[-1] + interp_gt = interpolate.interp1d(ldt, lsp) + + # Creating a distance traveled with equally spaced points - 2 meters distance + gt_dist = np.arange(50, int(round(dist_travelled - 50)), 2) + lsp_dist = interp_gt(gt_dist) + + # Compute acceleration based on the speed diff and the distance traveled + lap_dist, ltp_dist = [], [0] + for i in range(1, len(lsp_dist)): + lap_dist.append((lsp_dist[i] - lsp_dist[i - 1]) * (lsp_dist[i - 1] + lsp_dist[i]) / 4) + ltp_dist.append(4 / (lsp_dist[i - 1] + lsp_dist[i])) + lap_dist.append(0) + lap_dist = np.array(lap_dist) + ltp_dist = np.cumsum(np.array(ltp_dist)) + ldt_dist = gt_dist + + # Interpolation to get the distance cycle at each point + cycle_dist_cubic = interpolate.CubicSpline(ldt[1:], lsp[:-1]) + + # Dictionary for the lmfit optimization + min_lmfit = {} + tmpResDict = collections.OrderedDict() + tmpResDict['GroundTruth'] = {} + for model_name in Models: + model = Models[model_name] + instance = { + 'vn': 0, ### Desired speed of the follower in m/s + 'bn': bn, ### Maximum deceleration of the follower in m/s2 + 'an': np.random.uniform(0, 8), ### Maximum acceleration of the follower in m/s2 + 's0': 1, ### Minimum distance in meters + 'rt': rt, ### Reaction time in sec + 'car_id': car_id, + 'car': car, ### The car for the MFC + 'hyd_mode': hyd_mode, + 'veh_load': 75 * 0, + 'will_acc_model': 'horizontal_b', ### Will to accelerate + 'overshoot': 0, ### overshoot - not used + 'automatic': car.transmission, ### Gear box + 'mfc_curve': False, + 'gs': False, + 'driver_style': np.random.uniform(0, 1), + 'gs_style': np.random.uniform(0, 1), + 'followDistance': True, + 'distance_cycle': cycle_dist_cubic, + 'distTravelLimit': dist_travelled, + 'interp_gt': interp_gt, + } + + ficticious_leader = [30] * len(ltp_long) + + # Start of the lmfit optimization + params_lmfit = Parameters() + if model == msim.follow_leader_gipps: + # params_lmfit.add('an', value=1, vary=True, min=0.5, max=4.0) + params_lmfit.add('an', value=1, vary=True, min=0.5, max=5.0) + params_lmfit.add('bn', value=bn, vary=False, min=-4.0, max=-1) + params_lmfit.add('rt', value=rt, vary=False, min=1, max=20) + params_lmfit.add('beta', value=beta, vary=True, min=0.001, max=5.0) + params_lmfit.add('gamma', value=gamma, vary=True, min=0.1, max=2.0) + if model == msim.follow_leader_idm: + # params_lmfit.add('an', value=1, vary=True, min=0.5, max=4.0) + params_lmfit.add('an', value=1, vary=True, min=0.5, max=5.0) + params_lmfit.add('delta', value=delta, vary=True, min=0.1, max=10.0) + params_lmfit.add('bn', value=bn, vary=False, min=-4.0, max=-1) + if model == msim.follow_leader_mfc: + # We need first to run Gipps to calibrate an, bn and rt + params_lmfit.add('driver_style', value=1, vary=True, min=0.1, max=1) + params_lmfit.add('gs_style', value=1, vary=True, min=0.1, max=1) + params_lmfit.add('bn', value=bn, vary=False, min=-4.0, max=-1) + params_lmfit.add('rt', value=rt, vary=False, min=1, max=20) + + min_lmfit[model_name] = minimize(msim.fcn2minGA_freeflow, params_lmfit, + args=( + instance, model, ltp_long, ficticious_leader, start_dist, start_speed), + method='Powell', tol=10e-5) + + lsp_dist_cycle = cycle_dist_cubic(gt_dist) + # Plot the fitted models - LMFit + fig1 = plt.figure(figsize=(8, 6)) + plt.plot(ldt_dist, lsp_dist, label='ref. vehicle') + plt.plot(ldt_dist, lsp_dist_cycle, label='cycle') + plt.title('Distance - Speed (part of a lap)', fontsize=18) + df = pd.DataFrame() + df['ldt_dist'] = ldt_dist + df['lsp_dist'] = lsp_dist + df['lsp_dist_cycle'] = lsp_dist_cycle + df['ltp_dist'] = ltp_dist + + for model_name in Models: + mfit = min_lmfit[model_name] + model = Models[model_name] + Params = collections.OrderedDict() + if model == msim.follow_leader_gipps: + Params = { + 'an': mfit.params['an'].value, + 'rt': mfit.params['rt'].value, + 'bn': mfit.params['bn'].value, + 'beta': mfit.params['beta'].value, + 'gamma': mfit.params['gamma'].value, + } + if model == msim.follow_leader_idm: + Params = { + 'an': mfit.params['an'].value, + 'bn': mfit.params['bn'].value, + 'delta': mfit.params['delta'].value, + } + if model == msim.follow_leader_mfc: + Params = { + 'rt': mfit.params['rt'].value, + # 'bn': mfit.params['bn'].value, + 'driver_style': mfit.params['driver_style'].value, + 'gs_style': mfit.params['gs_style'].value, + } + + Params = collections.OrderedDict(sorted(Params.items())) + + tmpResDict[model_name] = Params + + new_instance = {**instance, **Params} + + sim_step = 0.1 + ficticious_leader = [30] * len(ltp_long) + m_sp = list(model(new_instance, ltp_long, ficticious_leader, sim_step, start_dist, start_speed)) + m_ap = np.append(np.diff(m_sp), 0) * 10 + # m_dt = list(np.cumsum([i * 0.1 + 0.005 * j for i, j in zip(m_sp, m_ap)])) + m_dt = list(np.cumsum([0] + [(m_sp[i]+m_sp[i-1]) / 2 * 0.1 for i in range(1, len(m_sp))])) + + interp_m = interpolate.interp1d(m_dt, m_sp) + + m_sp_1 = np.asarray(m_sp) + m_dt_1 = np.asarray(m_dt) + + # Store interpolation + Models_results[model_name]['interp'] = interp_m + + m_sp_dist = interp_m(gt_dist) + # m_ap_dist = np.append(np.diff(m_sp_dist), 0) * 10 + m_ap_dist, m_tp_dist = [], [0] + for i in range(1, len(m_sp_dist)): + m_ap_dist.append((m_sp_dist[i] - m_sp_dist[i - 1]) * ((m_sp_dist[i - 1] + m_sp_dist[i]) / 4)) + m_tp_dist.append(4 / (m_sp_dist[i - 1] + m_sp_dist[i])) + m_ap_dist.append(0) + m_ap_dist = np.array(m_ap_dist) + m_tp_dist = np.cumsum(np.array(m_tp_dist)) + m_dt_dist = gt_dist + + rmse_sp = np.sqrt(mean_squared_error(lsp_dist, m_sp_dist)) + rmse_ap = np.sqrt(mean_squared_error(lap_dist, m_ap_dist)) + rmse_tp = np.sqrt(mean_squared_error(ltp_dist, m_tp_dist)) + + similarity_sp = cosine_similarity(lsp_dist.reshape(1,-1), m_sp_dist.reshape(1,-1)) + + # Save rmsn errors + tmpResDict[model_name]['file'] = testDataset + tmpResDict[model_name]['lap'] = (lap_num + 1) + tmpResDict[model_name]['model'] = model_name + tmpResDict[model_name]['rmse_sp'] = rmse_sp + tmpResDict[model_name]['std_sp'] = np.std(np.array(lsp_dist) - np.array(m_sp_dist)) + tmpResDict[model_name]['rmse_ap'] = rmse_ap + tmpResDict[model_name]['std_ap'] = np.std(np.array(lap_dist) - np.array(m_ap_dist)) + tmpResDict[model_name]['rmse_tp'] = rmse_tp + tmpResDict[model_name]['std_tp'] = np.std(np.array(ltp_dist) - np.array(m_tp_dist)) + tmpResDict[model_name]['fit'] = mfit.residual[0] + + print(mfit.params.valuesdict()) + print(mfit.residual[0]) + print('rmse_speed: %.3f' % rmse_sp) + print('std_sp: %.3f' % np.std(np.array(lsp_dist) - np.array(m_sp_dist))) + print('rmse_acc: %.3f' % rmse_ap) + print('std_ap: %.3f' % np.std(np.array(lap_dist) - np.array(m_ap_dist))) + print('rmse_dist: %.3f' % rmse_tp) + print('std_tp: %.3f' % np.std(np.array(ltp_dist) - np.array(m_tp_dist))) + + Models_results[model_name]['sp'] = list(m_sp) + Models_results[model_name]['sp_dist'] = list(m_sp_dist) + Models_results[model_name]['ap_dist'] = list(m_ap_dist) + Models_results[model_name]['dt_dist'] = list(m_dt_dist) + Models_results[model_name]['tp_dist'] = list(m_tp_dist) + + df[model_name+'_sp_dist'] = Models_results[model_name]['sp_dist'] + df[model_name+'_ap_dist'] = Models_results[model_name]['ap_dist'] + df[model_name+'_dt_dist'] = Models_results[model_name]['dt_dist'] + df[model_name+'_tp_dist'] = Models_results[model_name]['tp_dist'] + + if model_name == 'Gipps': + linest = '-.' + elif model_name == 'IDM': + linest = ':' + else: + linest = '--' + + plt.plot(m_dt_dist, m_sp_dist, label=model_name, linestyle=linest) + plt.legend(loc='best') + plt.ylabel('Speed $(m/s)$', fontsize=16) + plt.xlabel('Distance $(m)$', fontsize=16) + + plt.savefig(os.path.join(cal_res_path, '%s_models_distance_speed_part.png' % testDataset), dpi=150) + plt.close(fig1) + df.to_csv(os.path.join(cal_res_path, '%s_models_part.csv' % testDataset), index=False) + + # Macro stats + print('Average speed GT:%.2f' % np.mean(lsp_dist)) + + print('Average absolute acceleration GT:%.2f' % (np.mean(np.absolute(lap_dist)))) + + # Save macro results + tmpResDict['GroundTruth']['avg_speed'] = np.mean(lsp_dist) + tmpResDict['GroundTruth']['avg_absolute_acceleration'] = np.mean(np.absolute(lap_dist)) + tmpResDict['GroundTruth']['duration'] = len(lsp) / freq + tmpResDict['GroundTruth']['file'] = testDataset + tmpResDict['GroundTruth']['model'] = 'GroundTruth' + + for model_name in Models: + m_sp_dist = Models_results[model_name]['sp_dist'] + print('Average speed %s:%.2f' % (model_name, np.mean(m_sp_dist))) + m_ap_dist = Models_results[model_name]['ap_dist'] + print('Average absolute acceleration %s:%.2f' % (model_name, np.mean(np.absolute(m_ap_dist)))) + m_sp = Models_results[model_name]['sp'] + print('Time dev %s:%d sec' % (model_name, (len(m_sp) - len(lsp)) / freq)) + + # Save macro results + tmpResDict[model_name]['avg_speed'] = np.mean(m_sp_dist) + tmpResDict[model_name]['avg_absolute_acceleration'] = np.mean(np.absolute(m_ap_dist)) + tmpResDict[model_name]['time_difference'] = (len(m_sp) - len(lsp)) / freq + + # Append the results for the specific file to the total results' list. + results_list.append(tmpResDict['GroundTruth']) + for model_name in Models: + results_list.append(tmpResDict[model_name]) + + # Plot of cycle - Acceleration + fig1, ax = plt.subplots(figsize=(8, 6)) + plt.plot(ldt_dist, lap_dist, label='ref. vehicle') + + # plt.plot(ldt, cycle_sp, label='cycle') + for model_name in Models: + m_dt_dist = Models_results[model_name]['dt_dist'] + m_ap_dist = Models_results[model_name]['ap_dist'] + if model_name == 'Gipps': + linest = '-.' + elif model_name == 'IDM': + linest = ':' + else: + linest = '--' + plt.plot(m_dt_dist, m_ap_dist, label=model_name, linestyle=linest) + + plt.ylabel('Acceleration $(m/s^2)$', fontsize=16) + plt.xlabel('Distance $(m)$', fontsize=16) + plt.title('Distance - Acceleration (part of a lap)', fontsize=18) + plt.legend(loc='best') + + plt.savefig(os.path.join(cal_res_path, '%s_models_distance_acceleration_part.png' % testDataset), dpi=150) + + plt.close(fig1) + + #################################################### + # Plot of cycle - Speed over Time + fig1, ax = plt.subplots() + plt.plot(ltp_dist, lsp_dist, label='ref. vehicle') + for model_name in Models: + m_tp_dist = Models_results[model_name]['tp_dist'] + m_sp_dist = Models_results[model_name]['sp_dist'] + plt.plot(m_tp_dist, m_sp_dist, label=model_name) + + plt.ylabel('Speed $(m/s)$') + plt.xlabel('Time $(s)$') + plt.title('Time - Speed (1 lap)') + plt.legend(loc='best') + # plt.show() + plt.savefig(os.path.join(cal_res_path, '%s_models_time_speed.png' % testDataset), dpi=300) + plt.close(fig1) + + # Plot of cycle - Acceleration over Time + fig1, ax = plt.subplots() + plt.plot(ltp_dist, lap_dist, label='ref. vehicle') + for model_name in Models: + m_tp_dist = Models_results[model_name]['tp_dist'] + m_ap_dist = Models_results[model_name]['ap_dist'] + plt.plot(m_tp_dist, m_ap_dist, label=model_name) + + plt.ylabel('Acceleration $(m/s^2)$') + plt.xlabel('Time $(s)$') + plt.title('Time - Acceleration (1 lap)') + plt.legend(loc='best') + # plt.show() + plt.savefig(os.path.join(cal_res_path, '%s_models_time_acceleration.png' % testDataset), dpi=300) + plt.close(fig1) + + if not results_list: + print('empty list') + else: + cal_res_df = pd.DataFrame(results_list) + cal_res_df.to_csv(os.path.join(cal_res_path, 'free_flow_calibration.csv'), + index=False) + + # Validation + results_list = [] + for lap_num, df in enumerate([df1, df2]): + testDataset = hyd_mode + '_' + str(lap_num+1) + if lap_num+1 == 1: + CalibrateDataset = hyd_mode + '_2' + elif lap_num+1 == 2: + CalibrateDataset = hyd_mode + '_1' + + ltp = np.array(df['Time [s]']-df['Time [s]'].iloc[0]) + lsp = np.array(df['Speed [m/s]']) + lap = np.array(df['Accel [m/s2]']) + + ltp_long = np.arange(0, round(2 * len(lsp) / 10, 1), 0.1) + freq = 10 + + # This test is about car following + start_dist = 10000 + start_speed = lsp[0] + bn = -3 + rt = 1 + beta = 0.025 + gamma = 0.5 + delta = 4 + + # ldp = [i * 0.1 + 0.005 * j for i, j in zip(lsp, lap)] + ldp = [0] + [(lsp[i]+lsp[i-1]) / 2 * 0.1 for i in range(1, len(lsp))] + ldt = np.cumsum(ldp) + dist_travelled = ldt[-1] + interp_gt = interpolate.interp1d(ldt, lsp) + + # Creating a distance traveled with equally spaced points - 2 meters distance + gt_dist = np.arange(50, int(round(dist_travelled - 50)), 2) + lsp_dist = interp_gt(gt_dist) + + # Compute acceleration based on the speed diff and the distance traveled + lap_dist, ltp_dist = [], [0] + for i in range(1, len(lsp_dist)): + lap_dist.append((lsp_dist[i] - lsp_dist[i - 1]) * (lsp_dist[i - 1] + lsp_dist[i]) / 4) + ltp_dist.append(4 / (lsp_dist[i - 1] + lsp_dist[i])) + lap_dist.append(0) + lap_dist = np.array(lap_dist) + ltp_dist = np.cumsum(np.array(ltp_dist)) + ldt_dist = gt_dist + + # Interpolation to get the distance cycle at each point + cycle_dist_cubic = interpolate.CubicSpline(ldt[1:], lsp[:-1]) + + # Save results + tmpResDict = collections.OrderedDict() + tmpResDict['GroundTruth'] = {} + + lsp_dist_cycle = cycle_dist_cubic(gt_dist) + fig_dist_speed = plt.figure() + ax_dist_speed = fig_dist_speed.add_subplot(311) + ax_dist_speed.plot(ldt_dist, lsp_dist, label='ref. vehicle') + ax_dist_speed.plot(ldt_dist, lsp_dist_cycle, label='cycle') + ax_dist_speed.set_title('Distance - Speed (1 lap)') + ax_dist_speed.set_ylabel('Speed $(m/s)$') + ax_dist_speed.set_xlabel('Distance $(m)$') + + ax_dist_acc = fig_dist_speed.add_subplot(312) + ax_dist_acc.plot(ldt_dist, lap_dist, label='ref. vehicle') + ax_dist_acc.set_title('Distance - Acceleration (1 lap)') + ax_dist_acc.set_ylabel('Acceleration $(m/s^2)$') + ax_dist_acc.set_xlabel('Distance $(m)$') + + ax_time_speed = fig_dist_speed.add_subplot(313) + ax_time_speed.plot(ltp, lsp, label='ref. vehicle') + ax_time_speed.set_title('Time - Speed (1 lap)') + ax_time_speed.set_ylabel('Speed $(m/s)$') + ax_time_speed.set_xlabel('Time $(s)$') + + df = pd.DataFrame() + df['ldt_dist'] = ldt_dist + df['lsp_dist'] = lsp_dist + df['lsp_dist_cycle'] = lsp_dist_cycle + df['lap_dist'] = lap_dist + # df['ltp'] = ltp + # df['lsp'] = lsp + + for model_name in Models: + model = Models[model_name] + instance = { + 'vn': 0, ### Desired speed of the follower in m/s + 'bn': bn, ### Maximum deceleration of the follower in m/s2 + 'an': np.random.uniform(0, 8), ### Maximum acceleration of the follower in m/s2 + 's0': 1, ### Minimum distance in meters + 'rt': rt, ### Reaction time in sec + 'car_id': car_id, + 'car': car, ### The car for the MFC + 'hyd_mode': hyd_mode, + 'veh_load': 75 * 0, + 'will_acc_model': 'horizontal_b', ### Will to accelerate + 'overshoot': 0, ### overshoot - not used + 'automatic': car.transmission, ### Gear box + 'mfc_curve': False, + 'gs': False, + 'driver_style': np.random.uniform(0, 1), + 'gs_style': np.random.uniform(0, 1), + 'followDistance': True, + 'distance_cycle': cycle_dist_cubic, + 'distTravelLimit': dist_travelled, + 'interp_gt': interp_gt, + } + + ficticious_leader = [30] * len(ltp_long) + Params = collections.OrderedDict() + if model == msim.follow_leader_gipps: + Params = { + 'an': cal_res_df['an'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'rt': cal_res_df['rt'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'bn': cal_res_df['bn'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'beta': cal_res_df['beta'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'gamma': cal_res_df['gamma'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + } + if model == msim.follow_leader_idm: + Params = { + 'an': cal_res_df['an'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'bn': cal_res_df['bn'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'delta': cal_res_df['delta'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + } + if model == msim.follow_leader_mfc: + # We need first to run Gipps to calibrate an, bn and rt + Params = { + 'rt': cal_res_df['rt'].loc[(cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'driver_style': cal_res_df['driver_style'].loc[ + (cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + 'gs_style': cal_res_df['gs_style'].loc[ + (cal_res_df.model == model_name) & (cal_res_df.file == CalibrateDataset)].values[0], + } + + Params = collections.OrderedDict(sorted(Params.items())) + + # Save the parameters used for validation + tmpResDict[model_name] = Params + + new_instance = {**instance, **Params} + + sim_step = 0.1 + m_sp = list(model(new_instance, ltp_long, ficticious_leader, sim_step, start_dist, start_speed)) + m_ap = np.append(np.diff(m_sp), 0) * 10 + # m_dt = list(np.cumsum([i * 0.1 + 0.005 * j for i, j in zip(m_sp, m_ap)])) + m_dt = list(np.cumsum([0] + [(m_sp[i]+m_sp[i-1]) / 2 * 0.1 for i in range(1, len(m_sp))])) + + interp_m = interpolate.interp1d(m_dt, m_sp) + + m_sp_1 = np.asarray(m_sp) + m_dt_1 = np.asarray(m_dt) + + m_sp_dist = interp_m(gt_dist) + m_ap_dist = np.append(np.diff(m_sp_dist), 0) * 10 + m_ap_dist, m_tp_dist = [], [0] + for i in range(1, len(m_sp_dist)): + m_ap_dist.append((m_sp_dist[i] - m_sp_dist[i - 1]) * ((m_sp_dist[i - 1] + m_sp_dist[i]) / 4)) + m_tp_dist.append(4 / (m_sp_dist[i - 1] + m_sp_dist[i])) + m_ap_dist.append(0) + m_ap_dist = np.array(m_ap_dist) + m_tp_dist = np.cumsum(np.array(m_tp_dist)) + m_dt_dist = gt_dist + + rmse_sp = np.sqrt(mean_squared_error(lsp_dist, m_sp_dist)) + rmse_ap = np.sqrt(mean_squared_error(lap_dist, m_ap_dist)) + rmse_tp = np.sqrt(mean_squared_error(ltp_dist, m_tp_dist)) + + rel_speed = [k / j for k, j in zip(m_sp_dist, lsp_dist)] + opt_metric = np.sum([np.log(k) ** 2 for k in rel_speed]) + + # Save rmsn errors + tmpResDict[model_name]['file'] = testDataset + tmpResDict[model_name]['model'] = model_name + tmpResDict[model_name]['rmse_sp'] = rmse_sp + tmpResDict[model_name]['std_sp'] = np.std(np.array(lsp_dist) - np.array(m_sp_dist)) + tmpResDict[model_name]['rmse_ap'] = rmse_ap + tmpResDict[model_name]['std_ap'] = np.std(np.array(lap_dist) - np.array(m_ap_dist)) + tmpResDict[model_name]['rmse_tp'] = rmse_tp + tmpResDict[model_name]['std_tp'] = np.std(np.array(ltp_dist) - np.array(m_tp_dist)) + tmpResDict[model_name]['fit'] = opt_metric + + print('Average speed %s:%.2f' % (model_name, np.mean(m_sp_dist))) + print('Average absolute acceleration %s:%.2f' % (model_name, np.mean(np.absolute(m_ap_dist)))) + print('Time dev %s:%.2f sec' % (model_name, (len(m_sp) - len(lsp)) / freq)) + + # Save macro results + tmpResDict[model_name]['avg_speed'] = np.mean(m_sp_dist) + tmpResDict[model_name]['avg_absolute_acceleration'] = np.mean(np.absolute(m_ap_dist)) + tmpResDict[model_name]['time_difference'] = (len(m_sp) - len(lsp)) / freq + + print('fit: %.3d' % opt_metric) + print('rmse_speed: %.3f' % rmse_sp) + print('rmse_acc: %.3f' % rmse_ap) + print('rmse_dist: %.3f' % rmse_tp) + ax_dist_speed.plot(m_dt_dist, m_sp_dist, label='%s RMSE(time):%.3f'%(model_name, rmse_tp)) + ax_dist_acc.plot(m_dt_dist, m_ap_dist, label='%s RMSE(time):%.3f'%(model_name, rmse_tp)) + ax_time_speed.plot(m_tp_dist, m_sp_dist, label='%s RMSE(time):%.3f'%(model_name, rmse_tp)) + + df[model_name+'_m_dt_dist'] = m_dt_dist + df[model_name+'_m_sp_dist'] = m_sp_dist + df[model_name+'_m_ap_dist'] = m_ap_dist + df[model_name+'_m_tp_dist'] = m_tp_dist + + fig_dist_speed.subplots_adjust(left=0.10, bottom=0.10, right=0.90, top=0.95, hspace=0.20) + ax_dist_speed.legend(bbox_to_anchor=(1.1, 1.05)) + ax_dist_acc.legend(bbox_to_anchor=(1.1, 1.05)) + ax_time_speed.legend(bbox_to_anchor=(1.1, 1.05)) + + plt.tight_layout() + # plt.show() + plt.savefig(os.path.join(val_res_path, '%s_models_distance_speed.png' % testDataset), dpi=300) + plt.close(fig_dist_speed) + + # Macro stats + print('Average speed GT:%.2f' % np.mean(lsp_dist)) + print('Average absolute acceleration GT:%.2f' % (np.mean(np.absolute(lap_dist)))) + + # Save macro results + tmpResDict['GroundTruth']['avg_speed'] = np.mean(lsp_dist) + tmpResDict['GroundTruth']['avg_absolute_acceleration'] = np.mean(np.absolute(lap_dist)) + tmpResDict['GroundTruth']['duration'] = len(lsp) / freq + tmpResDict['GroundTruth']['file'] = testDataset + tmpResDict['GroundTruth']['model'] = 'GroundTruth' + + # Append the results for the specific file to the total results' list. + results_list.append(tmpResDict['GroundTruth']) + for model_name in Models: + results_list.append(tmpResDict[model_name]) + + if not results_list: + print('empty list') + else: + val_res_df = pd.DataFrame(results_list) + val_res_df.to_csv(os.path.join(val_res_path, 'free_flow_validation.csv'), + index=False) + +# %% +if __name__ == "__main__": + car_id = 26712 # Niro, https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-executiveline-specs/76106 + db_name = os.path.join(pathRoot, 'c2art_env', 'sim_env', 'hybrid_mfc', + 'car_database', '2019_07_03_car_db_full') + db = rno.load_db_to_dictionary(db_name) + car = rno.get_vehicle_from_db(db, car_id) + + data_path = os.path.join(pathRoot, 'c2art_env', 'sim_env', 'hybrid_mfc', + 'datasets', 'kia_niro') + trajDataset = [ + '20190424_02_WarmUpCD_ElasticityCD_SailingCD_ConstSpeedCS_aligned.csv', + '20190424_03_ElasticityCS_SailingCS_aligned.csv'] + + hyd_mode = 'CD' + res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, hyd_mode, 'traj_cal_val') + if not os.path.exists(res_path): + os.makedirs(res_path, exist_ok=True) + df_cd = data_preprocess(data_path, trajDataset[0], hyd_mode, res_path) + df_cd_a = df_cd.loc[(df_cd['Time [s]']>=1310) & (df_cd['Time [s]']<=2010)] + plt_driving_traj(df_cd_a, os.path.join(res_path, 'traj_CD_a.png')) + df_cd_b = df_cd.loc[(df_cd['Time [s]']>=720) & (df_cd['Time [s]']<=920)] + plt_driving_traj(df_cd_b, os.path.join(res_path, 'traj_CD_b.png')) + + cal_val_traj(df_cd_a, df_cd_b, car, hyd_mode, res_path) + + + hyd_mode = 'CS' + res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, hyd_mode, 'traj_cal_val') + if not os.path.exists(res_path): + os.makedirs(res_path, exist_ok=True) + df_cs = data_preprocess(data_path, trajDataset[1], hyd_mode, res_path) + df_cs_a = df_cs.loc[(df_cs['Time [s]']>=1160) & (df_cs['Time [s]']<=1790)] + plt_driving_traj(df_cs_a, os.path.join(res_path, 'traj_CS_a.png')) + df_cs_b = df_cs.loc[(df_cs['Time [s]']>=850) & (df_cs['Time [s]']<=1115)] + plt_driving_traj(df_cs_b, os.path.join(res_path, 'traj_CS_b.png')) + + cal_val_traj(df_cs_a, df_cs_b, car, hyd_mode, res_path) + +# %% diff --git a/MAIN_hybridMFC/MAIN_hybrid_mfc_dc.py b/MAIN_hybridMFC/MAIN_hybrid_mfc_dc.py new file mode 100644 index 0000000..16a7cd4 --- /dev/null +++ b/MAIN_hybridMFC/MAIN_hybrid_mfc_dc.py @@ -0,0 +1,127 @@ +# %% +# Packages & paths +import sys, os, time +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +import multiprocessing +from itertools import product +from functools import partial +from IPython.core import display as ICD + +pathRoot = os.path.abspath(__file__ + "/../../") +sys.path.append(pathRoot) + +import c2art_env.sim_env.hybrid_mfc.reading_n_organizing as rno +import c2art_env.sim_env.hybrid_mfc.microsim as msim + + +def mfc_main( + car, + car_id, + hyd_mode, + veh_load, + driver_style, + driver_style_dc, + gs_style, + res_path + ): + + car_info, acc_curve, dec_curve, veh_model_speed, veh_model_acc, veh_model_dec, veh_max_speed, gs_th \ + = msim.mfc_curves( + car, + car_id, + hyd_mode, + veh_load, + gs_style, + res_path, + ) + + df = pd.DataFrame() + df['Spd [m/s]'] = veh_model_speed + df['Accel [m/s2]'] = veh_model_acc + df['Decel [m/s2]'] = veh_model_dec + df.to_csv(os.path.join(res_path, hyd_mode+'_MFC_curves.csv'), index=False) + # """ + msim.plt_exp_val_spd_accel( + pathRoot, + car_id, + car_info, + hyd_mode, + veh_model_speed, + veh_model_acc, + veh_model_dec, + veh_max_speed, + res_path + ) + # """ + + msim.plt_accel_scenario( + driver_style, + gs_th, + acc_curve, + dec_curve, + veh_max_speed, + car_info, + car, + hyd_mode, + res_path + ) + + msim.plt_accel_scenario_dc( + driver_style_dc, + acc_curve, + dec_curve, + veh_max_speed, + car_info, + res_path + ) + + +if __name__ == "__main__": + # Select valid hybrid specs from the car database + db_name = os.path.join(pathRoot, 'c2art_env', 'sim_env', 'hybrid_mfc', + 'car_database', '2019_07_03_car_db_full') + # %% + # Select the car and model its driving capabilities + # 26966 - Space Star, https://www.cars-data.com/en/mitsubishi-space-star-1.2-life-specs/73285 + # 7565 - Golf, https://www.cars-data.com/en/volkswagen-golf-2.0-tdi-150hp-highline-specs/59579 + # 26687 - Ioniq, https://www.cars-data.com/en/hyundai-ioniq-electric-comfort-specs/76078 + # 26712 - Niro, https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-executiveline-specs/76106 + + cars = [ + # 0 - 'ICEV' + [26966, 7565], + # 1 - 'EV' + [26687], + # 2 - 'HEV' + [26712], + ] + + car_id = cars[2][0] # SELECT THE CAR + hyd_mode = ['Na', 'CD', 'CS'][2] # CD/CS, ONLY VALID FOR HEV, Na = not applicable + + driver_style = [1.0, 0.8, 0.6] + driver_style_dc = [0, 1, 2] # Class driver_charact() in c2art_env/sim_env/hybrid_mfc/driver_charact.py + gs_style = [0.9, 0.7, 0.5] + veh_load = 75 * 0 + + db = rno.load_db_to_dictionary(db_name) + car = rno.get_vehicle_from_db(db, car_id) + if hyd_mode == 'Na': + res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, 'curves') + else: + res_path = os.path.join(pathRoot, 'PostProcessing', 'HybridMFC', 'Outputs', car.model, hyd_mode, 'curves') + if not os.path.exists(res_path): + os.makedirs(res_path, exist_ok=True) + + mfc_main( + car, + car_id, + hyd_mode, + veh_load, + driver_style, + driver_style_dc, + gs_style[2], + res_path + ) diff --git a/MAIN_platoon_mpc.m b/MAIN_platoon_mpc.m new file mode 100644 index 0000000..70ab7cc --- /dev/null +++ b/MAIN_platoon_mpc.m @@ -0,0 +1,473 @@ +close all +clear +clc + +% [33333, 1458, 8422, 50010] +carIDs = int64([33333, 33333, 1458, 8422, 50010, 1458, 8422, 50010]); % Including the platoon leader +numVeh = length(carIDs); % The number of vehicles in a platoon (incl. the leader) +numFoll = numVeh-1; +timeStep = 0.1; % Time step +timeSim = 10; % Trip duration +numStep = floor(timeSim/timeStep); % Simulation setps +Np = 20; % Number of prediction horizon (steps) +d = 20; % Desired spacing +Time = (0.1:timeStep:timeSim); + +[a_max, a_min, f_0, f_1, f_2, phi, tau_a, veh_mass, mfc_array, mfc_slice] = parameter_init(carIDs(2:end)); + +% ['PF', 'PLF', 'BD', 'BDL', 'TPF', 'TPLF'] +[matA, matP] = communication_init(numFoll, 'TPLF'); + +[Position, Velocity, Acceleration, U, p0, v0, a0, Pa, Va, Aa, ua,... + Pa_next, Va_next, Aa_next, ua_next, Pend, Vend, Aend, Cost, Exitflg]... + = variable_init(numFoll, numStep, timeStep, Np, d, f_0, f_1, f_2,... + phi, tau_a, veh_mass, mfc_array, mfc_slice); + +for i = 2:numStep-Np + fprintf('\n Steps i= %d\n',i) + for j = 1:numFoll + tic + sin_theta = 0; + + [vehType, mfc_curve, X0, R, Xa, F, numXdes, Xdes, Q, numXn, Xn, G, lb, ub, u0, Pnp, Vnp, Anp]... + = mpc_step_init(i, j, Np, d, numFoll, Position, Velocity, Acceleration,... + a_max, a_min, f_0, f_1, f_2, phi, tau_a, veh_mass, mfc_array, mfc_slice,... + Pa, Va, Aa, ua, matA, matP, p0, v0, a0); + + Pend(j, i) = Pnp; + Vend(j, i) = Vnp; + Aend(j, i) = Anp; + + tol_opt = 1e-5; + options = optimset('Display','off', 'TolFun', tol_opt, 'MaxIter', 2000,... + 'LargeScale', 'off', 'RelLineSrchBnd', [], 'RelLineSrchBndDuration', 1); + A = [];b = []; Aeq = []; beq = []; + [u, Cost(j, i), Exitflg(j, i), output] = fmincon(... + @(u) cost_func(u, Np, timeStep, sin_theta, X0, vehType, mfc_curve, Q, Xdes, R, F, Xa, G, Xn, numXn),... + u0, A, b, Aeq, beq, lb, ub, @(u) nonlcon_func(u, Np, timeStep, sin_theta, X0, vehType, mfc_curve, Pnp, Vnp, Anp),options); + + [U, Position, Velocity, Acceleration, Pa_next, Va_next, Aa_next, ua] = assume_update(i, j, Np, u, U, ua, Position, Velocity, Acceleration,... + timeStep, sin_theta, vehType, mfc_curve, Pa_next, Va_next, Aa_next); + + toc + end + + Pa = Pa_next; + Va = Va_next; + Aa = Aa_next; + +end + +ResPlot(numFoll, numStep, Np, d, U, Position, Velocity, Acceleration, p0, v0, a0) + + +function ResPlot(numFoll, numStep, Np, d, U, Position, Velocity, Acceleration, p0, v0, a0) + +figure; +plot(p0(1:numStep-Np) - Position(1, 1:numStep-Np), 'DisplayName', 'Spacing_1');hold on; +for k = 2:numFoll + plot(Position(k-1, 1:numStep-Np) - Position(k, 1:numStep-Np), 'DisplayName', ['Spacing_' num2str(k)]);hold on; +end +legend +xlabel('Step'); +ylabel('Spacing (m)'); + +figure; +for k = 1:numFoll + plot(Position(k, 1:numStep-Np) - (p0(1:numStep-Np) - k*d), 'DisplayName', ['ErrSpacLead_' num2str(k)]);hold on; +end +legend +xlabel('Step'); +ylabel('Lead spacing error (m)'); + +figure; +plot(Position(1, 1:numStep-Np) - (p0(1:numStep-Np) - d), 'DisplayName', 'ErrSpacNbr_1');hold on; +for k = 2:numFoll + plot(Position(k, 1:numStep-Np) - (Position(k-1, 1:numStep-Np) - d), 'DisplayName', ['ErrSpacNbr_' num2str(k)]);hold on; +end +legend +xlabel('Step'); +ylabel('Neighbor spacing error (m)'); + +figure; +plot(v0(1:numStep-Np), 'DisplayName', 'v_0');hold on; +for k = 1:numFoll + plot(Velocity(k, 1:numStep-Np), 'DisplayName', ['v_' num2str(k)]);hold on; +end +legend +xlabel('Step'); +ylabel('Speed (m/s)'); + +figure; +plot(a0(1:numStep-Np), 'DisplayName', 'a_0');hold on; +for k = 1:numFoll + plot(Acceleration(k, 1:numStep-Np), 'DisplayName', ['a_' num2str(k)]);hold on; +end +legend +xlabel('Step'); +ylabel('Acceleration (m/s^2)'); + +figure; +for k = 1:numFoll + plot(U(k, 1:numStep-Np), 'DisplayName', ['U_' num2str(k)]);hold on; +end +legend +xlabel('Step'); +ylabel('U - acceleration (m/s^2)'); + +end + + +function [U, Position, Velocity, Acceleration, Pa_next, Va_next, Aa_next, ua] = assume_update(i, j, Np, u, U, ua, Position, Velocity, Acceleration,... + timeStep, sin_theta, vehType, mfc_curve, Pa_next, Va_next, Aa_next) + +f_0 = vehType(1); +f_1 = vehType(2); +f_2 = vehType(3); +phi = vehType(4); +tau_a = vehType(5); +veh_mass = vehType(6); +mfc_curve = mat2npndarray(mfc_curve); + +U(j, i) = u(1); + +RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.vehicle_dynamic(... + U(j, i),... + Position(j, i-1),... + Velocity(j, i-1),... + Acceleration(j, i-1),... + timeStep,... + sin_theta,... + f_0,... + f_1,... + f_2,... + phi,... + tau_a,... + veh_mass,... + mfc_curve); + +Position(j, i) = RES{1}; +Velocity(j, i) = RES{2}; +Acceleration(j, i) = RES{3}; + +Temp = zeros(3, Np+1); +Temp(:, 1) = [Position(j, i); Velocity(j, i); Acceleration(j, i)]; +ua(j, 1:Np-1) = u(2:Np); +for k = 1:Np-1 + RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.vehicle_dynamic(... + ua(j, k),... + Temp(1, k),... + Temp(2, k),... + Temp(3, k),... + timeStep,... + sin_theta,... + f_0,... + f_1,... + f_2,... + phi,... + tau_a,... + veh_mass,... + mfc_curve); + Temp(1, k+1) = RES{1}; + Temp(2, k+1) = RES{2}; + Temp(3, k+1) = RES{3}; +end + +ua(j, Np) = (f_0 * (1 - sin_theta^2)^0.5 + f_1 * Temp(2, Np) + f_2 * Temp(2, Np)^2) / veh_mass + 9.80665 * sin_theta; + +RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.vehicle_dynamic(... + ua(j, Np),... + Temp(1, Np),... + Temp(2, Np),... + Temp(3, Np),... + timeStep,... + sin_theta,... + f_0,... + f_1,... + f_2,... + phi,... + tau_a,... + veh_mass,... + mfc_curve); + +Temp(1, Np+1) = RES{1}; +Temp(2, Np+1) = RES{2}; +Temp(3, Np+1) = RES{3}; + +Pa_next(j,:) = Temp(1,:); +Va_next(j,:) = Temp(2,:); +Aa_next(j,:) = Temp(3,:); + +end + + +function [C, Ceq] = nonlcon_func(u, Np, timeStep, sin_theta, X0, vehType, mfc_curve, Pnp, Vnp, Anp) + +u = mat2np1darray(u); +Np = int16(Np); +% timeStep +% sin_theta +X0 = mat2np1darray(X0); +vehType = mat2np1darray(vehType); +mfc_curve = mat2npndarray(mfc_curve); +% Pnp +% Vnp +% Anp + +RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.nonlcon_func(... + u,... + Np,... + timeStep,... + sin_theta,... + X0,... + vehType,... + mfc_curve,... + Pnp,... + Vnp,... + Anp); + +C = []; +Ceq = nparray2mat(RES); + +end + + +function cost = cost_func(u, Np, timeStep, sin_theta, X0, vehType, mfc_curve, Q, Xdes, R, F, Xa, G, Xn, numXn) + +u = mat2np1darray(u); +Np = int16(Np); +% timeStep +% sin_theta +X0 = mat2np1darray(X0); +vehType = mat2np1darray(vehType); +mfc_curve = mat2npndarray(mfc_curve); +Q = mat2npndarray(Q); +Xdes = mat2npndarray(Xdes); +% R +F = mat2npndarray(F); +Xa = mat2npndarray(Xa); +G = mat2npndarray(G); +Xn = mat2npndarray(Xn); +numXn = int16(numXn); + +RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.cost_func(... + u,... + Np,... + timeStep,... + sin_theta,... + X0,... + vehType,... + mfc_curve,... + Q,... + Xdes,... + R,... + F,... + Xa,... + G,... + Xn,... + numXn); + +cost = RES; + +end + + +function [vehType, mfc_curve, X0, R, Xa, F, numXdes, Xdes, Q, numXn, Xn, G, lb, ub, u0, Pnp, Vnp, Anp] = mpc_step_init(... + i, j, Np, d, numFoll, Position, Velocity, Acceleration, a_max, a_min, f_0, f_1, f_2, ... + phi, tau_a, veh_mass, mfc_array, mfc_slice, Pa, Va, Aa, ua, matA, matP, p0, v0, a0) + +vehType = [f_0(j), f_1(j), f_2(j), phi(j), tau_a(j), veh_mass(j)]; +mfc_curve = mfc_array(:, mfc_slice(j)+1:mfc_slice(j+1)); + +X0 = [Position(j, i-1), Velocity(j, i-1), Acceleration(j, i-1)]; + +% Input-deviation +R = 0.1; + +% Self-deviation +Xa = [Pa(j, :); Va(j, :); Aa(j, :)]; +F = diag([5.0, 2.5, 1.0]) .* (sum(matA(:, j))+1)^2; + +% Leader-deviation +if matP(j, j) == 1 + numXdes = 1; + Pd = p0(i-1:i+Np-1) - j*d; + Vd = v0(i-1:i+Np-1); + Ad = a0(i-1:i+Np-1); + Xdes = [Pd; Vd; Ad]; + Q = diag([5.0, 2.5, 1.0]); +else + numXdes = 0; + Xdes = zeros(3, Np+1); + Q = diag([0.0, 0.0, 0.0]); +end + +% Neighbor-deviation +numXn = sum(matA(j, :)); +if numXn == 0 + Xn = zeros(3, Np+1); + G = diag([0.0, 0.0, 0.0]); +else + Xn = zeros(0, Np+1); + for k = 1:numFoll + if matA(j,k) == 1 + Xn = [Xn; Pa(k,:) - (j-k)*d; Va(k,:); Aa(k,:)]; + end + G = diag([5.0, 2.5, 1.0]); + end +end + +lb = a_min(j) * ones(1,Np); +ub = a_max(j) * ones(1,Np); +u0 = ua(j,:); + +Pnp = Xdes(1, end); +Vnp = Xdes(2, end); +Anp = Xdes(3, end); + +for k = 1:numXn + Pnp = Pnp + Xn(3*k-2, end); + Vnp = Vnp + Xn(3*k-1, end); + Anp = Anp + Xn(3*k, end); +end +Pnp = Pnp/(numXdes+numXn); +Vnp = Vnp/(numXdes+numXn); +Anp = Anp/(numXdes+numXn); + +end + + +function [Position, Velocity, Acceleration, U, p0, v0, a0, Pa, Va, Aa, ua, Pa_next, Va_next, Aa_next, ua_next, Pend, Vend, Aend, Cost, Exitflg] = variable_init(numFoll, numStep, timeStep, Np, d, f_0, f_1, f_2, phi, tau_a, veh_mass, mfc_array, mfc_slice) + +numFoll = int16(numFoll); +numStep = int16(numStep); +% timeStep +Np = int16(Np); +% d +f_0 = mat2np1darray(f_0); +f_1 = mat2np1darray(f_1); +f_2 = mat2np1darray(f_2); +phi = mat2np1darray(phi); +tau_a = mat2np1darray(tau_a); +veh_mass = mat2np1darray(veh_mass); +mfc_array = mat2npndarray(mfc_array); +mfc_slice = mat2np1darray(mfc_slice); + +RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.variable_init(... + numFoll, numStep, timeStep, Np, d, f_0, f_1, f_2,... + phi, tau_a, veh_mass, mfc_array, mfc_slice); + +Position = nparray2mat(RES{1}); +Velocity = nparray2mat(RES{2}); +Acceleration = nparray2mat(RES{3}); +U = nparray2mat(RES{4}); +p0 = nparray2mat(RES{5}); +v0 = nparray2mat(RES{6}); +a0 = nparray2mat(RES{7}); +Pa = nparray2mat(RES{8}); +Va = nparray2mat(RES{9}); +Aa = nparray2mat(RES{10}); +ua = nparray2mat(RES{11}); +Pa_next = nparray2mat(RES{12}); +Va_next = nparray2mat(RES{13}); +Aa_next = nparray2mat(RES{14}); +ua_next = nparray2mat(RES{15}); +Pend = nparray2mat(RES{16}); +Vend = nparray2mat(RES{17}); +Aend = nparray2mat(RES{18}); +Cost = nparray2mat(RES{19}); +Exitflg = nparray2mat(RES{20}); + +end + + +function [matA, matP] = communication_init(numFoll, TopoType) + +numFoll = int16(numFoll); + +% ['PF', 'PLF', 'BD', 'BDL', 'TPF', 'TPLF'] +RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.communication_init(numFoll, TopoType); +matA = nparray2mat(RES{1}); +matP = nparray2mat(RES{2}); + +end + + +function [a_max, a_min, f_0, f_1, f_2, phi, tau_a, veh_mass, mfc_array, mfc_slice] = parameter_init(follIDs) + +% ['PF', 'PLF', 'BD', 'BDL', 'TPF', 'TPLF'] +RES = py.c2art_env.sim_env.platooning_mpc.setup_utils.parameter_init(follIDs); + +a_max = nparray2mat(RES{1}); +a_min = nparray2mat(RES{2}); +f_0 = nparray2mat(RES{3}); +f_1 = nparray2mat(RES{4}); +f_2 = nparray2mat(RES{5}); +phi = nparray2mat(RES{6}); +tau_a = nparray2mat(RES{7}); +veh_mass = nparray2mat(RES{8}); +mfc_array = nparray2mat(RES{9}); +mfc_slice = int16(nparray2mat(RES{10})); + +end + + +function result = mat2np1darray( matarray ) + +result=py.numpy.array(matarray); + +end + + +function result = mat2npndarray( matarray ) + %mat2nparray Convert a Matlab array into an nparray + % Convert an n-dimensional Matlab array into an equivalent nparray + data_size=size(matarray); + if length(data_size)==1 + % 1-D vectors are trivial + result=py.numpy.array(matarray); + elseif length(data_size)==2 + % A transpose operation is required either in Matlab, or in Python due + % to the difference between row major and column major ordering + transpose=matarray'; + % Pass the array to Python as a vector, and then reshape to the correct + % size + result=py.numpy.reshape(transpose(:)', int32(data_size)); + else + % For an n-dimensional array, transpose the first two dimensions to + % sort the storage ordering issue + transpose=permute(matarray,[length(data_size):-1:1]); + % Pass it to python, and then reshape to the python style of matrix + % sizing + result=py.numpy.reshape(transpose(:)', int32(fliplr(size(transpose)))); + end +end + + +function result = nparray2mat( nparray ) + %nparray2mat Convert an nparray from numpy to a Matlab array + % Convert an n-dimensional nparray into an equivalent Matlab array + data_size = cellfun(@int64,cell(nparray.shape)); + if length(data_size)==1 + % This is a simple operation + result=double(py.array.array('d', py.numpy.nditer(nparray))); + elseif length(data_size)==2 + % order='F' is used to get data in column-major order (as in Fortran + % 'F' and Matlab) + result=reshape(double(py.array.array('d', ... + py.numpy.nditer(nparray, pyargs('order', 'F')))), ... + data_size); + else + % For multidimensional arrays more manipulation is required + % First recover in python order (C contiguous order) + result=double(py.array.array('d', ... + py.numpy.nditer(nparray, pyargs('order', 'C')))); + % Switch the order of the dimensions (as Python views this in the + % opposite order to Matlab) and reshape to the corresponding C-like + % array + result=reshape(result,fliplr(data_size)); + % Now transpose rows and columns of the 2D sub-arrays to arrive at the + % correct Matlab structuring + result=permute(result,[length(data_size):-1:1]); + end +end \ No newline at end of file diff --git a/MAIN_platoon_mpc.py b/MAIN_platoon_mpc.py new file mode 100644 index 0000000..5c5c350 --- /dev/null +++ b/MAIN_platoon_mpc.py @@ -0,0 +1,124 @@ +import os, math, time, datetime +import matplotlib.pyplot as plt +import pandas as pd +import numpy as np +from numba import njit, types +from numba.typed import Dict +from scipy.optimize import minimize, NonlinearConstraint, Bounds +import networkx as nx +from c2art_env.veh_model.mfc.mfc_constraints import mfc_curves +from c2art_env.veh_model.longitudinal_dyn_veh import nonlinear_lon_dyn, linear_lon_dyn, none_lon_dyn +from c2art_env import utils +from c2art_env.sim_env.platooning_mpc import setup_utils as su + + +if __name__ == '__main__': + + ### [33333, 1458, 8422, 50010] ### + carIDs = [33333, 33333, 1458, 8422, 50010, 1458, 8422, 50010] # Including the platoon leader + numVeh = len(carIDs) # The number of vehicles in a platoon (incl. the leader) + timeStep = 0.1 # Time step + timeSim = 10 # Trip duration + numStep = math.floor(timeSim/timeStep) # Simulation setps + Np = 20 # Number of prediction horizon (steps) + d = 20 # Desired spacing + Time = np.arange(0, timeSim, timeStep) + + a_max, a_min, f_0, f_1, f_2, phi, tau_a, veh_mass, mfc_array, mfc_slice \ + = su.parameter_init(carIDs[1:]) + + ### ['PF', 'PLF', 'BD', 'BDL', 'TPF', 'TPLF'] ### + matA, matP = su.communication_init(numVeh-1, 'TPLF') + + U, Postion, Velocity, Acceleration, p0, v0, a0, \ + Pend, Vend, Aend = su.mpc_simulation( + d, + Np, + numVeh, + numStep, + timeStep, + # Communication topology + matA, + matP, + # Vehicle type + a_max, + a_min, + f_0, + f_1, + f_2, + veh_mass, + phi, + tau_a, + mfc_array, + mfc_slice + ) + + col_ = ['Time'] + ['P'+str(m) for m in range(numVeh)] + \ + ['V'+str(m) for m in range(numVeh)] + ['A'+str(m) for m in range(numVeh)] + \ + ['U'+str(m) for m in range(1, numVeh)] + + data_ = np.vstack((Time, p0, Postion, v0, Velocity, a0, Acceleration, U)) + + df_ = pd.DataFrame(data=data_[:,:-Np].transpose(), columns=col_) + if True: + df_.to_csv('mpc_time_series.csv', index=False) + + if True: + plt.figure() + plt.plot(a0[:-Np]) + for i in range(numVeh-1): + plt.plot(Acceleration[i][:-Np]) + plt.title('Acceleration') + plt.show() + plt.close() + + plt.figure() + plt.plot(v0[:-Np]) + for i in range(numVeh-1): + plt.plot(Velocity[i][:-Np]) + plt.title('Velocity') + plt.show() + plt.close() + + plt.figure() + for i in range(numVeh-1): + plt.plot(Postion[i][:-Np] - (p0[:-Np] - (i+1)*d)) + plt.title('LEAD Pos Error') + plt.show() + plt.close() + + plt.figure() + plt.plot(p0[:-Np] - d - Postion[0][:-Np]) + for i in range(1, numVeh-1): + plt.plot(Postion[i-1][:-Np] - d - Postion[i][:-Np]) + plt.title('NRB Pos Error') + plt.show() + plt.close() + + plt.figure() + for i in range(numVeh-1): + plt.plot(U[i][:-Np]) + plt.title('U') + plt.show() + plt.close() + + plt.figure() + for i in range(numVeh-1): + plt.plot(Pend[i][1:-Np]) + plt.title('Pend') + plt.show() + plt.close() + + plt.figure() + for i in range(numVeh-1): + plt.plot(Vend[i][1:-Np]) + plt.title('Vend') + plt.show() + plt.close() + + plt.figure() + for i in range(numVeh-1): + plt.plot(Aend[i][1:-Np]) + plt.title('Aend') + plt.show() + plt.close() diff --git a/README.md b/README.md index a7f6bae..5057e02 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,29 @@ # c2art-car-following-env +# MFC-based Platoon simulation with V2V communication and distributed model predictive control + +## REFERENCES + +### Platoon with model predictive control + +[1] [Zheng Y, Li SE, Li K, Borrelli F, Hedrick JK. Distributed model predictive control for heterogeneous vehicle platoons under unidirectional topologies. IEEE Transactions on Control Systems Technology. 2016 Aug 17;25(3):899-910.](https://ieeexplore.ieee.org/document/7546918) + +[2] [Li K, Bian Y, Li SE, Xu B, Wang J. Distributed model predictive control of multi-vehicle systems with switching communication topologies. Transportation Research Part C: Emerging Technologies. 2020 Sep 1;118:102717.](https://www.sciencedirect.com/science/article/pii/S0968090X2030632X) + +[3] [Li SE, Qin X, Zheng Y, Wang J, Li K, Zhang H. Distributed platoon control under topologies with complex eigenvalues: Stability analysis and controller synthesis. IEEE Transactions on Control Systems Technology. 2017 Nov 10;27(1):206-20.](https://ieeexplore.ieee.org/abstract/document/8103336) + +### MFC car-following model + +[4] [He Y, Makridis M, Mattas K, Fontaras G, Ciuffo B, Xu H. Introducing Electrified Vehicle Dynamics in Traffic Simulation. Transportation Research Record. 2020 Sep;2674(9):776-91.](https://journals.sagepub.com/doi/full/10.1177/0361198120931842) + +[5] [Makridis M, Fontaras G, Ciuffo B, Mattas K. MFC free-flow model: Introducing vehicle dynamics in microsimulation. Transportation Research Record. 2019 Apr;2673(4):762-77.](https://journals.sagepub.com/doi/full/10.1177/0361198119838515) + +## How to use + +1. (Recommended) MATLAB: Run the script of 'MAIN_platoon_mpc.m' in MATLAB + +2. (Not recommended) PYTHON: Run the script of 'MAIN_platoon_mpc.py' in PYTHON. (I have not yet finalized this script because the solver (scipy.optimize.minimize) is unstable and not optimal in this case.) + +pyenv('Version', "C:\Users\leiti\miniconda3\envs\jrc\python.exe") + +conda env export > environment.yml diff --git a/c2art_env/sim_env/car_following/setup_utils.py b/c2art_env/sim_env/car_following/setup_utils.py index e31da8e..d647b53 100644 --- a/c2art_env/sim_env/car_following/setup_utils.py +++ b/c2art_env/sim_env/car_following/setup_utils.py @@ -494,7 +494,7 @@ def optimization( CalExp_Names = [Exp_Names[CalExpIDX]] - CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values) # ID used to search car specs. + CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values[0]) # ID used to search car specs. parm_cstr, parm_cstr_mfc = setup_constraints(pathInput, CalVehID, CarID) @@ -624,7 +624,7 @@ def validation( ###################################################### # Setup parameters (validation only), cstr, and data # ###################################################### - CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values) # ID used to search car specs. + CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values[0]) # ID used to search car specs. parm_cstr, parm_cstr_mfc = setup_constraints(pathInput, CalVehID, CarID) diff --git a/c2art_env/sim_env/free_flow_stable/setup_utils.py b/c2art_env/sim_env/free_flow_stable/setup_utils.py index 1bf1eed..752c281 100644 --- a/c2art_env/sim_env/free_flow_stable/setup_utils.py +++ b/c2art_env/sim_env/free_flow_stable/setup_utils.py @@ -410,7 +410,7 @@ def optimization( CalExp_Names = Exp_Names[CalExpIDX] - CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values) # ID used to search car specs. + CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values[0]) # ID used to search car specs. parm_cstr, parm_cstr_mfc = setup_constraints(pathInput, CalVehID, CarID) @@ -539,7 +539,7 @@ def validation( ###################################################### # Setup parameters (validation only), cstr, and data # ###################################################### - CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values) # ID used to search car specs. + CarID = int(CarMap.loc[CarMap[0] == CalVehID, 1].values[0]) # ID used to search car specs. parm_cstr, parm_cstr_mfc = setup_constraints(pathInput, CalVehID, CarID) diff --git a/c2art_env/sim_env/hybrid_mfc/car_database/2019_07_03_car_db_full.csv b/c2art_env/sim_env/hybrid_mfc/car_database/2019_07_03_car_db_full.csv index a3b1776..a2cb99b 100644 --- a/c2art_env/sim_env/hybrid_mfc/car_database/2019_07_03_car_db_full.csv +++ b/c2art_env/sim_env/hybrid_mfc/car_database/2019_07_03_car_db_full.csv @@ -26701,7 +26701,7 @@ 26709,https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-first-edition-specs/76101,Kia Niro 1.6 GDi Hybrid First Edition,,28495,suv/crossover,automatic,5,2016,2017,front,hybrid,petrol,104,265,4,4,1580,13,77,5700,147,4000,injection,dohc,no,regular,45,synchronous motor ,32,170,1,lithium-ion,1.56,240,,,162,11.5,3.8,3.9,3.8,88,A,3.23,2400,1400,1930,,4.355,1.805,1.545,2.7,0.16,yes,yes,no,no,yes,yes,yes,yes,yes,yes,yes,no,no,yes,5,97,326.2,316.6990291,[3.87- 2.22- 1.37- 0.93- 0.96- 0.77],1400,1475,1500,1470,2.788725 26710,https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-businessline-specs/76104,Kia Niro 1.6 GDi Hybrid BusinessLine,,30495,suv/crossover,automatic,5,2016,available,front,hybrid,petrol,104,265,4,4,1580,13,77,5700,147,4000,injection,dohc,no,regular,45,synchronous motor ,32,170,1,lithium-ion,1.56,240,,,162,11.5,3.8,3.9,3.8,88,A,3.23,2400,1400,1930,,4.355,1.805,1.545,2.7,0.16,yes,yes,yes,no,yes,yes,yes,yes,yes,no,no,no,no,yes,5,97,326.2,316.6990291,[3.87- 2.22- 1.37- 0.93- 0.96- 0.77],1400,1475,1500,1470,2.788725 26711,https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-sportsline-specs/76105,Kia Niro 1.6 GDi Hybrid SportsLine,,32595,suv/crossover,automatic,5,2016,available,front,hybrid,petrol,104,265,4,4,1580,13,77,5700,147,4000,injection,dohc,no,regular,45,synchronous motor ,32,170,1,lithium-ion,1.56,240,,,162,11.5,4.4,4.5,4.4,101,A,3.23,2400,1421,1930,,4.355,1.805,1.545,2.7,0.16,yes,yes,yes,no,yes,yes,yes,yes,yes,no,no,no,no,yes,5,97,329.85,320.2427184,[3.87- 2.22- 1.37- 0.93- 0.96- 0.77],1421,1496,1521,1470,2.788725 -26712,https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-executiveline-specs/76106,Kia Niro 1.6 GDi Hybrid ExecutiveLine,,33095,suv/crossover,automatic,5,2016,available,front,hybrid,petrol,104,265,4,4,1580,13,77,5700,147,4000,injection,dohc,no,regular,45,synchronous motor ,32,170,1,lithium-ion,1.56,240,,,162,11.5,4.4,4.5,4.4,101,A,3.23,2400,1421,1930,,4.355,1.805,1.545,2.7,0.16,yes,yes,yes,no,yes,yes,yes,yes,yes,no,no,no,no,yes,5,97,329.85,320.2427184,[3.87- 2.22- 1.37- 0.93- 0.96- 0.77],1421,1496,1521,1470,2.788725 +26712,https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-executiveline-specs/76106,Kia Niro 1.6 GDi Hybrid ExecutiveLine,,33095,suv/crossover,automatic,5,2016,available,front,hybrid,petrol,104,265,4,4,1580,13,77,5700,147,4000,injection,dohc,no,regular,45,synchronous motor ,32,170,1,lithium-ion,1.56,240,,,162,11.5,4.4,4.5,4.4,101,A,3.23,2400,1421,1930,,4.355,1.805,1.545,2.7,0.16,yes,yes,yes,no,yes,yes,yes,yes,yes,no,no,no,no,yes,5,97,329.85,320.2427184,[3.87- 2.22- 1.37- 0.96- 0.93- 0.77],1421,1496,1521,1470,2.788725 26713,https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-dynamicline-specs/76102,Kia Niro 1.6 GDi Hybrid DynamicLine,,28495,suv/crossover,automatic,5,2017,available,front,hybrid,petrol,104,265,4,4,1580,13,77,5700,147,4000,injection,dohc,no,regular,45,synchronous motor ,32,170,1,lithium-ion,1.56,240,,,162,11.5,3.8,3.9,3.8,88,A,3.23,2400,1400,1930,,4.355,1.805,1.545,2.7,0.16,yes,yes,no,no,yes,yes,yes,yes,yes,yes,yes,no,no,yes,5,97,326.2,316.6990291,[3.87- 2.22- 1.37- 0.93- 0.96- 0.77],1400,1475,1500,1470,2.788725 26714,https://www.cars-data.com/en/kia-niro-1.6-gdi-hybrid-style-edition-specs/76103,Kia Niro 1.6 GDi Hybrid Style Edition,,29790,suv/crossover,automatic,5,2017,available,front,hybrid,petrol,104,265,4,4,1580,13,77,5700,147,4000,injection,dohc,no,regular,45,synchronous motor ,32,170,1,lithium-ion,1.56,240,,,162,11.5,3.8,3.9,3.8,88,A,3.23,2400,1400,1930,,4.355,1.805,1.545,2.7,0.16,yes,yes,no,no,yes,yes,yes,yes,yes,yes,yes,no,no,yes,5,97,326.2,316.6990291,[3.87- 2.22- 1.37- 0.93- 0.96- 0.77],1400,1475,1500,1470,2.788725 26715,https://www.cars-data.com/en/kia-optima-sw-2.0-t-gdi-gt-specs/76107,Kia Optima SW 2.0 T-GDi GT,,57495,stationwagon,automatic,5,2016,available,front,fuel engine,petrol,180,353,4,4,1998,10,180,6000,353,1350,injection,dohc,yes,regular,70,,,,,,,,,,232,7.6,11.8,6.1,8.2,191,F,2.89,2150,1580,2190,100,4.855,1.86,1.46,2.805,0.125,yes,yes,yes,yes,yes,yes,yes,yes,yes,yes,yes,yes,yes,yes,5,86,329.85,320.2427184,[4.77- 2.95- 1.92- 1.42- 1.00- 0.77],1580,1655,1680,1700,2.7156 @@ -55547,3 +55547,4 @@ 55556,https://www.cars-data.com/en/toyota-yaris-1.5-full-hybrid-now-specs/70089,Toyota Yaris 1.5 Full Hybrid Now,B,18440,hatchback,,5,2015,available,front,hybrid,petrol,74,169,4,4,1497,13.4,55,4800,111,3600,injection,dohc,no,regular,36,synchronous motor ,45,169,2,nickel-metal hydride,,,,,165,12,3.3,3.1,3.3,75,A,,,1060,1565,50,3.905,1.695,1.51,2.51,,yes,yes,,no,yes,yes,no,yes,yes,no,no,no,no,yes,5,84.7,304.25,295.3883495,,1060,1135,1160,1130,2.55945 55557,https://www.cars-data.com/en/volkswagen-crosspolo-1.2-tsi-90hp-specs/68161,Volkswagen CrossPolo 1.2 TSI 90hp,,20890,hatchback,manual,5,2014,available,front,fuel engine,petrol,66,160,4,4,1197,10,66,4800,160,1400,injection,dohc,yes,regular,45,,,,,,,,,,177,11.4,6,4.2,4.9,109,C,3.63,2850,1065,1580,75,3.987,1.698,1.488,2.469,0.175,yes,yes,,no,yes,yes,yes,yes,no,yes,244,no,no,yes,5,75.6,301.9,293.1067961,[3.77- 1.96- 1.28- 0.93- 0.74],1065,1140,1165,1130,2.526624 55558,https://www.cars-data.com/en/volkswagen-crosspolo-1.2-tsi-90hp-specs/68162,Volkswagen CrossPolo 1.2 TSI 90hp,,22890,hatchback,automatic,5,2014,available,front,fuel engine,petrol,66,160,4,4,1197,10,66,4800,160,1400,injection,dohc,yes,regular,45,,,,,,,,,,177,11.4,5.9,4.3,4.9,114,C,3.12,2550,1106,1620,75,3.987,1.698,1.488,2.469,0.175,yes,yes,,no,yes,yes,yes,yes,no,yes,244,no,no,yes,5,75.6,301.9,293.1067961,[3.77- 2.37- 1.58- 1.11- 1.14- 0.94- 0.78],1106,1181,1206,1130,2.526624 +55559,https://www.cars-data.com/en/volkswagen-golf-1-4-ehybrid-204hp-style-specs/166618,Volkswagen Golf 1.4 EHybrid 204hp Style 2020 Automatic 5 doors specs,C,39835,hatchback,automatic,5,2020,available,front,hybrid,petrol,150,,4,4,1395,,110,5000,250,1550,direct injection,dohc,yes,regular,40,synchronous motor ,70,330,1,lithium-ion,13,360,,,220,7.4,,,,,,1,,1629,1698,,4.284,1.789,1.482,2.629,0.16,yes,no,yes,,yes,yes,yes,yes,yes,yes,,,,yes,5,80,329.85,320.2427184,[13.125- 8.0- 5.343- 3.825- 2.951- 2.423],1629,1590,1590,1590, diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-21_08-13-36__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-21_08-13-36__processed_1.xlsx new file mode 100644 index 0000000..133b7a4 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-21_08-13-36__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-21_08-13-36__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-21_08-13-36__processed_2.xlsx new file mode 100644 index 0000000..d4d7110 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-21_08-13-36__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-24_08-19-48__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-24_08-19-48__processed_1.xlsx new file mode 100644 index 0000000..b2fd735 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-24_08-19-48__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-24_08-19-48__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-24_08-19-48__processed_2.xlsx new file mode 100644 index 0000000..29c2b35 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/291118_raw2022-01-24_08-19-48__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/aggregated_information_by_trip.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/aggregated_information_by_trip.xlsx new file mode 100644 index 0000000..362767f Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/aggregated_information_by_trip.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-02-25_12-07-24__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-02-25_12-07-24__processed_1.xlsx new file mode 100644 index 0000000..bc98679 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-02-25_12-07-24__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-03-26_17-13-18__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-03-26_17-13-18__processed_1.xlsx new file mode 100644 index 0000000..274fd83 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-03-26_17-13-18__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-04-18_17-11-22__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-04-18_17-11-22__processed_1.xlsx new file mode 100644 index 0000000..f0e4e07 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-04-18_17-11-22__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-06-08_18-49-06__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-06-08_18-49-06__processed_1.xlsx new file mode 100644 index 0000000..9ad20a4 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-06-08_18-49-06__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-06-10_14-28-30__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-06-10_14-28-30__processed_1.xlsx new file mode 100644 index 0000000..9858d5b Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-06-10_14-28-30__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-17_17-58-08__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-17_17-58-08__processed_1.xlsx new file mode 100644 index 0000000..3ff6c88 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-17_17-58-08__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_1.xlsx new file mode 100644 index 0000000..a50e46e Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_2.xlsx new file mode 100644 index 0000000..f05a299 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_3.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_3.xlsx new file mode 100644 index 0000000..08f2f0e Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-08-31_18-52-24__processed_3.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-09-22_11-19-24__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-09-22_11-19-24__processed_1.xlsx new file mode 100644 index 0000000..8c808bb Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-09-22_11-19-24__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-09-22_11-19-24__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-09-22_11-19-24__processed_2.xlsx new file mode 100644 index 0000000..2965388 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-09-22_11-19-24__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-01_08-09-48__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-01_08-09-48__processed_1.xlsx new file mode 100644 index 0000000..ce98ba2 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-01_08-09-48__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-27_19-11-00__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-27_19-11-00__processed_1.xlsx new file mode 100644 index 0000000..7d51ab0 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-27_19-11-00__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-27_19-11-00__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-27_19-11-00__processed_2.xlsx new file mode 100644 index 0000000..3a50359 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2022-12-27_19-11-00__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2023-01-07_13-33-44__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2023-01-07_13-33-44__processed_1.xlsx new file mode 100644 index 0000000..cf19ef7 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2023-01-07_13-33-44__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2023-02-05_16-26-16__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2023-02-05_16-26-16__processed_1.xlsx new file mode 100644 index 0000000..8d7da50 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile2023-02-05_16-26-16__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2021-12-17_20-57-32__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2021-12-17_20-57-32__processed_1.xlsx new file mode 100644 index 0000000..873052d Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2021-12-17_20-57-32__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_1.xlsx new file mode 100644 index 0000000..a24b630 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_2.xlsx new file mode 100644 index 0000000..ddfd0f2 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_3.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_3.xlsx new file mode 100644 index 0000000..0a5b142 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_3.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_4.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_4.xlsx new file mode 100644 index 0000000..5362c54 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-02-19_19-50-06__processed_4.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_1.xlsx new file mode 100644 index 0000000..8f47502 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_2.xlsx new file mode 100644 index 0000000..00c0408 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_3.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_3.xlsx new file mode 100644 index 0000000..e117ed5 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_3.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_4.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_4.xlsx new file mode 100644 index 0000000..a63a011 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_4.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_5.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_5.xlsx new file mode 100644 index 0000000..ed83a68 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_5.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_6.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_6.xlsx new file mode 100644 index 0000000..33024b3 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-01_17-55-56__processed_6.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-10_10-25-50__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-10_10-25-50__processed_1.xlsx new file mode 100644 index 0000000..2315cec Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-10_10-25-50__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-19_16-24-26__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-19_16-24-26__processed_1.xlsx new file mode 100644 index 0000000..887eeba Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-04-19_16-24-26__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_1.xlsx new file mode 100644 index 0000000..272ab93 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_2.xlsx new file mode 100644 index 0000000..8cd20be Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_3.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_3.xlsx new file mode 100644 index 0000000..b977f49 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-05-21_16-49-42__processed_3.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-07-18_08-55-40__processed_1.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-07-18_08-55-40__processed_1.xlsx new file mode 100644 index 0000000..7af4abe Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-07-18_08-55-40__processed_1.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-07-18_08-55-40__processed_2.xlsx b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-07-18_08-55-40__processed_2.xlsx new file mode 100644 index 0000000..c43c1d2 Binary files /dev/null and b/c2art_env/sim_env/hybrid_mfc/datasets/volkswagen_golf8/logfile_raw2022-07-18_08-55-40__processed_2.xlsx differ diff --git a/c2art_env/sim_env/hybrid_mfc/mfc_acc.py b/c2art_env/sim_env/hybrid_mfc/mfc_acc.py index 0523945..a419e59 100644 --- a/c2art_env/sim_env/hybrid_mfc/mfc_acc.py +++ b/c2art_env/sim_env/hybrid_mfc/mfc_acc.py @@ -266,7 +266,8 @@ def hybrid_curves( veh_mass, f0, f1, - f2): + f2, + hyd_mode): phi = car.phi max_power = car.max_power # kW engine_max_power = int(car.engine_max_power) # kW @@ -288,6 +289,8 @@ def hybrid_curves( idle_engine_speed ) full_load_torque = full_load_powers * 1000 * (full_load_speeds / 60 * 2 * np.pi) ** -1 + car.ice_full_load = [full_load_speeds, full_load_torque] # rpm, Nm + speed_per_gear, acc_per_gear = [], [] for j in range(len(gr)): speed_per_gear.append([]) @@ -332,6 +335,8 @@ def hybrid_curves( motor_full_load_torque.append(motor_max_torque) elif motor_full_load_speeds[k] > motor_base_speed: motor_full_load_torque.append((6e4 * motor_max_power) / (2 * np.pi * motor_full_load_speeds[k])) + car.em_full_load = np.array([motor_full_load_speeds, motor_full_load_torque]) # rpm, Nm + motor_speed_per_gear, motor_acc_per_gear = [], [] for j in range(len(gr)): motor_speed_per_gear.append([]) @@ -385,11 +390,14 @@ def hybrid_curves( em_acc[(sp_bins < start)] = 0 em_acc[(sp_bins > stop)] = 0 - tractive_acc = ice_acc + em_acc + if hyd_mode == "CS": + tractive_acc = ice_acc + em_acc + elif hyd_mode == "CD": + tractive_acc = em_acc tractive_acc[tractive_acc > Alimit] = Alimit final_acc = tractive_acc - car_res_curve(sp_bins) final_acc = final_acc / phi final_acc[final_acc < 0] = 0 Res.append(interp1d(sp_bins, final_acc)) - return Res, cs_acc_per_gear, (Start, Stop) + return Res, cs_acc_per_gear, (Start, Stop), car \ No newline at end of file diff --git a/c2art_env/sim_env/hybrid_mfc/microsim.py b/c2art_env/sim_env/hybrid_mfc/microsim.py index e288ff3..f061ef1 100644 --- a/c2art_env/sim_env/hybrid_mfc/microsim.py +++ b/c2art_env/sim_env/hybrid_mfc/microsim.py @@ -1,17 +1,393 @@ +import re, time import sys, os, math import numpy as np +from tqdm import tqdm +from scipy.interpolate import interp1d import pandas as pd +from numba import jit, njit, prange import matplotlib.pyplot as plt from scipy import interpolate, signal from lmfit import Parameters, minimize import c2art_env.utils as utils +import c2art_env.sim_env.hybrid_mfc.utils as sim_utils +from c2art_env.sim_env.hybrid_mfc.utils import PID from c2art_env.sim_env.hybrid_mfc.road_load_coefficients import compute_f_coefficients import c2art_env.sim_env.hybrid_mfc.mfc_acc as driver from c2art_env.sim_env.hybrid_mfc.driver_charact import driver_charact +pid_aecms_ef = PID(Kp=125, Ki=0.16, Kd=0.004) + + +def energy_consumption_cd( + car, + a, + v, + gear, + soc_p, + dt=0.1, +): + a_r = (car.f0 + car.f1 * v * 3.6 + car.f2 * pow(v * 3.6, 2)) / car.total_mass + a_t = a + a_r + trq_req = a_t * car.total_mass * car.tire_radius / car.driveline_efficiency / (car.final_drive * car.gr[gear]) # Debug=50 + w_em = v / (car.tire_radius * (1 - car.driveline_slippage)) * (car.final_drive * car.gr[gear]) # rad/s, Debug=315 + + # Motor and Generator Configuration + n_machine = 0.9 # Efficiency of electrical machine + # T_em_max = car.motor_max_torque # 400 # Maximum Electric machine Torque [Nm] + T_em_max = np.interp(w_em*60/(2*np.pi), car.em_full_load[0], car.em_full_load[1]) + # T_em_max = utils.interp_binary(car.em_full_load[0], car.em_full_load[1], w_ice*60/(2*np.pi)) + P_em_max = car.motor_max_power # 50 # Power of Electric Machine [kW] + m_em = 1.5 # Electric motor weight [kg/Kw] + + # Battery Configuration + Q_0 = car.battery_capacity*1000/car.battery_voltage # 6.5 # Battery charging capacity [Ah] + U_oc = car.battery_voltage # 300 # Open circuit voltage [V] + R_i = 0.65 # Inner resistance [ohm] + # P_batt_max = P_em_max / n_machine # [kW] + # I_batt_max = (U_oc - np.sqrt(max(0, (U_oc**2-4*R_i*P_batt_max*1000))))/(2*R_i) + I_max = 200 # Maximum dis-/charging current [A] + I_min = -200 # Maximum dis-/charging current [A] + m_batt = 45 # [kg] + + T_em = max(trq_req, -T_em_max) + Power_electric_machine = T_em*w_em + Power_battery = T_em * w_em / (n_machine**np.sign(T_em)) + I = (U_oc - np.sqrt(max(0, (U_oc**2-4*R_i*Power_battery))))/(2*R_i) + soc = -I*dt / (Q_0*3600) + soc_p + soc = sim_utils._clamp(soc, (0.0, 1.0)) + P_ech = I*U_oc # Electro-chemical Power for Battery + + return P_ech, soc, T_em, w_em, I + +# @njit(nogil=True) +def energy_consumption_cs_aecms( + car, + a, + v, + v_p, + gear, + soc_r_p, + soc_p, + dt=0.1, + ): + ef = pid_aecms_ef(soc_r_p, soc_p) # Debug= 1.5 Equivalence Factor (constant or variable) + # ef = 1.5 + a_r = (car.f0 + car.f1 * v * 3.6 + car.f2 * pow(v * 3.6, 2)) / car.total_mass + a_t = a + a_r + trq_req = a_t * car.total_mass * car.tire_radius / car.driveline_efficiency / (car.final_drive * car.gr[gear]) # Debug=50 + # w_ice_rpm = v / (2 * np.pi * car.tire_radius * (1 - car.driveline_slippage)) * (60 * car.final_drive * car.gr[gear]) # rpm + # https://www.engineeringtoolbox.com/angular-velocity-acceleration-power-torque-d_1397.html + # https://www.convertunits.com/from/RPM/to/rad/sec + w_ice = v / (car.tire_radius * (1 - car.driveline_slippage)) * (car.final_drive * car.gr[gear]) # rad/s, Debug=315 + w_ice_p = v_p / (car.tire_radius * (1 - car.driveline_slippage)) * (car.final_drive * car.gr[gear]) # rad/s + dw_ice = (w_ice - w_ice_p) / dt # Debug=300 + + # Fuel and Air Parameters + H_l = 44.6e6 # Lower Heating Value [J/kg] + roha_petrol = 737.2 # Fuel Density [Kg/m3] + roha_air = 1.18 # Air Density [kg/m3] + + # Engine Configuration + J_e = 0.2 # Engine Inertia [kgm2] + T_ice_max = car.engine_max_torque # 115 # Engine Maximum Torque [Nm] + # T_ice_max = np.interp(w_ice*60/(2*np.pi), car.ice_full_load[0], car.ice_full_load[1]) + # T_ice_max = utils.interp_binary(car.ice_full_load[0], car.ice_full_load[1], w_ice*60/(2*np.pi)) + P_ice_max = car.engine_max_power # Engine Maximum Power [kW] + V_d = car.fuel_eng_capacity*1e-6 # 1.497e-3 # Engine Displacment [m3] + n_r = 2 + m_e = 1.2 # [kg/KW] + + # Willans approximation of engine efficiency + e = 0.4 # Willans approximation of engine efficiency + p_me_0 = 0.1e6 # Willans approximation of engine pressure [MPa] + + # Motor and Generator Configuration + n_machine = 0.9 # Efficiency of electrical machine + T_em_max = car.motor_max_torque # 400 # Maximum Electric machine Torque [Nm] + # T_em_max = np.interp(w_ice*60/(2*np.pi), car.em_full_load[0], car.em_full_load[1]) + # T_em_max = utils.interp_binary(car.em_full_load[0], car.em_full_load[1], w_ice*60/(2*np.pi)) + P_em_max = car.motor_max_power # 50 # Power of Electric Machine [kW] + m_em = 1.5 # Electric motor weight [kg/Kw] + + # Battery Configuration + Q_0 = car.battery_capacity*1000/car.battery_voltage # 6.5 # Battery charging capacity [Ah] + U_oc = car.battery_voltage # 300 # Open circuit voltage [V] + R_i = 0.65 # Inner resistance [ohm] 0.2 (dimitris) + # P_batt_max = P_em_max / n_machine # [kW] + # I_batt_max = (U_oc - np.sqrt(max(0, (U_oc**2-4*R_i*P_batt_max*1000))))/(2*R_i) + I_max = 200 # Maximum dis-/charging current [A] + I_min = -200 # Maximum dis-/charging current [A] + m_batt = 45 # [kg] + + # Total Powertrain Power + P_total_max = car.max_power # 90.8 # Total powertrain power[kW] + + # Logitudinal Vehicle Model Parameters + g = 9.81 # Gravity [m/s2] + c_D = 0.32 # Drag coefficient [-] + c_R = 0.015 # Rolling resistance coefficient [-] + A_f = 2.31 # Frontal area [m2] + m = car.total_mass # 1500 # Vehicle mass [kg] + r_w = car.tire_radius # 0.3 # Wheel radius [m] + J_w = 0.6 # Inertia of the wheels [kgm2] + m_wheel = J_w/(r_w**2) # Mass of Wheel [kg] + n_gearbox = 0.98 # Efficiency of Transmission + + # Range + N_e = np.arange(0, 5000, 800) # Engine RPM range + w_e = 300 # [rad/s2] % Maximum accelration of the engine + + # -----------------------Cost Vector Calculations-------------------------- + # Battery Model + + # I = np.linspace(I_min, I_max, 50000) + # Power_battery = (U_oc*I) - (R_i*(I**2)) # Power of Battery + + # Electric Machine Model + # Power_electric_machine = Power_battery*(n_machine**np.sign(I)) # Power of Electric Machine + + # T_em = (Power_battery*(n_machine**np.sign(I))) / w_ice # Torque of Electric Machine + # T_em[T_em > T_em_max] = T_em_max + # T_em[T_em < -T_em_max] = -T_em_max + + T_em = np.linspace(-T_em_max, T_em_max, 50000) + Power_electric_machine = T_em*w_ice + Power_battery = T_em * w_ice / (n_machine**np.sign(T_em)) + tmp_ = U_oc**2-4*R_i*Power_battery + tmp_[tmp_<0]=0 + I = (U_oc - np.sqrt(tmp_))/(2*R_i) + # Engine Model + # trq_req = max(trq_req, -T_em_max) + T_ice = trq_req - T_em # Torque of Engine + # T_ice[T_ice<0] = 0 + + x = ((p_me_0*V_d)/(4*np.pi)) # First Part to calculate Fuel consumption + + y = J_e*dw_ice # Second Part to calculate Fuel consumption + + costVector = (w_ice/(e*H_l))*(T_ice+x+y) # Fuel Consumption Cost Vector for given t_vec + + # Hamiltonian Calculation [Torque Split] + + P_f = H_l*costVector # Fuel Power + P_f[costVector<0] = 0 # Constraints + P_ech = I*U_oc # Electro-chemical Power for Battery + + # If condition for torque split + if w_ice<=0.1: + T_ice_cmd = 0 + T_em_cmd = 0 + u = [T_ice_cmd, T_em_cmd] + Batt_I=0 + soc = soc_p + p_ice = 0 + p_em = 0 + p_batt = 0 + p_fuel = 0 + fc = 0 + else: + H = P_f + (ef * P_ech) + H[(T_ice > T_ice_max)] = np.inf + H[(I > I_max) | (I < I_min)] = np.inf + # H[(T_em > T_em_max) | (T_em < -T_em_max)] = np.inf + H[(Power_electric_machine < -P_em_max*1000) | (Power_electric_machine > P_em_max*1000)] = np.inf + i = np.argmin(H) + T_ice_cmd = max(0, T_ice[i]) + T_em_cmd = T_em[i] + u = [T_ice_cmd, T_em_cmd] + # New battery state of charge + Batt_I = I[i] + soc = -Batt_I*dt / (Q_0*3600) + soc_p + soc = sim_utils._clamp(soc, (0.0, 1.0)) + # Power and fuel economy + p_ice = T_ice_cmd*w_ice + p_em = T_em_cmd*w_ice + p_batt = P_ech[i] # Energy consumption of battery [W] + p_fuel = P_f[i] # Energy consumption of ice [W] + fc = p_fuel*dt/H_l/roha_petrol*1000 # Fuel consumption [L] + + return fc, p_fuel, p_batt, p_ice, p_em, T_ice_cmd, T_em_cmd, soc, w_ice, trq_req, ef, Batt_I + + +def powertrain_hyd_cs_aecms( + car, + soc_p, + soc_ref_p, + w_gr_in, + trq_gr_in, + dw_ice, + dt=0.1, + ): + ef = pid_aecms_ef(soc_ref_p, soc_p) # Debug= 1.5 Equivalence Factor (constant or variable) + + # Fuel and Air Parameters + H_l = 44.6e6 # Lower Heating Value [J/kg] + roha_petrol = 737.2 # Fuel Density [Kg/m3] + roha_air = 1.18 # Air Density [kg/m3] + + # Engine Configuration + J_e = 0.2 # Engine Inertia [kgm2] + T_ice_max = car.engine_max_torque # 115 # Engine Maximum Torque [Nm] + # T_ice_max = np.interp(w_ice*60/(2*np.pi), car.ice_full_load[0], car.ice_full_load[1]) + # T_ice_max = utils.interp_binary(car.ice_full_load[0], car.ice_full_load[1], w_ice*60/(2*np.pi)) + P_ice_max = car.engine_max_power # Engine Maximum Power [kW] + V_d = car.fuel_eng_capacity*1e-6 # 1.497e-3 # Engine Displacment [m3] + n_r = 2 + m_e = 1.2 # [kg/KW] + + # Willans approximation of engine efficiency + e = 0.4 # Willans approximation of engine efficiency + p_me_0 = 0.1e6 # Willans approximation of engine pressure [MPa] + + # Motor and Generator Configuration + n_machine = 0.9 # Efficiency of electrical machine + T_em_max = car.motor_max_torque # 400 # Maximum Electric machine Torque [Nm] + # T_em_max = np.interp(w_ice*60/(2*np.pi), car.em_full_load[0], car.em_full_load[1]) + # T_em_max = utils.interp_binary(car.em_full_load[0], car.em_full_load[1], w_ice*60/(2*np.pi)) + P_em_max = car.motor_max_power # 50 # Power of Electric Machine [kW] + m_em = 1.5 # Electric motor weight [kg/Kw] + + # Battery Configuration + Q_0 = car.battery_capacity*1000/car.battery_voltage # 6.5 # Battery charging capacity [Ah] + U_oc = car.battery_voltage # 300 # Open circuit voltage [V] + R_i = 0.65 # Inner resistance [ohm] 0.2 (dimitris) + # P_batt_max = P_em_max / n_machine # [kW] + # I_batt_max = (U_oc - np.sqrt(max(0, (U_oc**2-4*R_i*P_batt_max*1000))))/(2*R_i) + I_max = 200 # Maximum dis-/charging current [A] + I_min = -200 # Maximum dis-/charging current [A] + m_batt = 45 # [kg] + + # Total Powertrain Power + P_total_max = car.max_power # 90.8 # Total powertrain power[kW] + + # Logitudinal Vehicle Model Parameters + g = 9.81 # Gravity [m/s2] + c_D = 0.32 # Drag coefficient [-] + c_R = 0.015 # Rolling resistance coefficient [-] + A_f = 2.31 # Frontal area [m2] + m = car.total_mass # 1500 # Vehicle mass [kg] + r_w = car.tire_radius # 0.3 # Wheel radius [m] + J_w = 0.6 # Inertia of the wheels [kgm2] + m_wheel = J_w/(r_w**2) # Mass of Wheel [kg] + n_gearbox = 0.98 # Efficiency of Transmission + + # Range + N_e = np.arange(0, 5000, 800) # Engine RPM range + w_e = 300 # [rad/s2] % Maximum accelration of the engine + + # -----------------------Cost Vector Calculations-------------------------- + # Battery Model + + # I = np.linspace(I_min, I_max, 50000) + # Power_battery = (U_oc*I) - (R_i*(I**2)) # Power of Battery + + # Electric Machine Model + # Power_electric_machine = Power_battery*(n_machine**np.sign(I)) # Power of Electric Machine + + # T_em = (Power_battery*(n_machine**np.sign(I))) / w_ice # Torque of Electric Machine + # T_em[T_em > T_em_max] = T_em_max + # T_em[T_em < -T_em_max] = -T_em_max + + T_em = np.linspace(-T_em_max, T_em_max, 50000) + Power_electric_machine = T_em*w_gr_in + Power_battery = T_em * w_gr_in / (n_machine**np.sign(T_em)) + tmp_ = U_oc**2-4*R_i*Power_battery + tmp_[tmp_<0]=0 + I = (U_oc - np.sqrt(tmp_))/(2*R_i) + # Engine Model + # trq_req = max(trq_req, -T_em_max) + T_ice = trq_gr_in - T_em # Torque of Engine + # T_ice[T_ice<0] = 0 + + x = ((p_me_0*V_d)/(4*np.pi)) # First Part to calculate Fuel consumption + + y = J_e*dw_ice # Second Part to calculate Fuel consumption + + costVector = (w_gr_in/(e*H_l))*(T_ice+x+y) # Fuel Consumption Cost Vector for given t_vec + + # Hamiltonian Calculation [Torque Split] + + P_f = H_l*costVector # Fuel Power + P_f[costVector<0] = 0 # Constraints + P_ech = I*U_oc # Electro-chemical Power for Battery + + # If condition for torque split + if w_gr_in<=0.1: + T_ice_cmd = 0 + T_em_cmd = 0 + u = [T_ice_cmd, T_em_cmd] + Batt_I=0 + soc = soc_p + p_ice = 0 + p_em = 0 + p_batt = 0 + p_fuel = 0 + fc = 0 + else: + H = P_f + (ef * P_ech) + H[(T_ice > T_ice_max)] = np.inf + H[(I > I_max) | (I < I_min)] = np.inf + # H[(T_em > T_em_max) | (T_em < -T_em_max)] = np.inf + H[(Power_electric_machine < -P_em_max*1000) | (Power_electric_machine > P_em_max*1000)] = np.inf + i = np.argmin(H) + T_ice_cmd = max(0, T_ice[i]) + T_em_cmd = T_em[i] + u = [T_ice_cmd, T_em_cmd] + # New battery state of charge + Batt_I = I[i] + soc = -Batt_I*dt / (Q_0*3600) + soc_p + soc = sim_utils._clamp(soc, (0.0, 1.0)) + # Power and fuel economy + p_ice = T_ice_cmd*w_gr_in + p_em = T_em_cmd*w_gr_in + p_batt = P_ech[i] # Energy consumption of battery [W] + p_fuel = P_f[i] # Energy consumption of ice [W] + fc = p_fuel*dt/H_l/roha_petrol*1000 # Fuel consumption [L] + + return soc, fc, T_ice_cmd, p_ice, p_fuel, T_em_cmd, p_em, p_batt, Batt_I, ef + + +def powertrain_hyd_cd( + car, + soc_p, + w_gr_in, + trq_gr_in, + dt=0.1, + ): + # Motor and Generator Configuration + n_machine = 0.9 # Efficiency of electrical machine + # T_em_max = car.motor_max_torque # 400 # Maximum Electric machine Torque [Nm] + T_em_max = np.interp(w_gr_in*60/(2*np.pi), car.em_full_load[0], car.em_full_load[1]) + # T_em_max = utils.interp_binary(car.em_full_load[0], car.em_full_load[1], w_ice*60/(2*np.pi)) + P_em_max = car.motor_max_power # 50 # Power of Electric Machine [kW] + m_em = 1.5 # Electric motor weight [kg/Kw] + + # Battery Configuration + Q_0 = car.battery_capacity*1000/car.battery_voltage # 6.5 # Battery charging capacity [Ah] + U_oc = car.battery_voltage # 300 # Open circuit voltage [V] + R_i = 0.65 # Inner resistance [ohm] + # P_batt_max = P_em_max / n_machine # [kW] + # I_batt_max = (U_oc - np.sqrt(max(0, (U_oc**2-4*R_i*P_batt_max*1000))))/(2*R_i) + I_max = 200 # Maximum dis-/charging current [A] + I_min = -200 # Maximum dis-/charging current [A] + m_batt = 45 # [kg] + + T_em = max(trq_gr_in, -T_em_max) + p_em = T_em*w_gr_in + Power_battery = T_em * w_gr_in / (n_machine**np.sign(T_em)) + I = (U_oc - np.sqrt(max(0, (U_oc**2-4*R_i*Power_battery))))/(2*R_i) + soc = -I*dt / (Q_0*3600) + soc_p + soc = sim_utils._clamp(soc, (0.0, 1.0)) + P_ech = I*U_oc # Electro-chemical Power for Battery + + return soc, T_em, p_em, P_ech, I + def variable_speed_sim( + car, + hyd_mode, driver_style, + gs_th, ap_curve, dp_curve, will_acc_model, @@ -25,8 +401,15 @@ def variable_speed_sim( X, V, A, V_n = np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)) V_n[0] = 20 + FC, Gear, SOC, SOC_R = np.zeros(len(t)), np.zeros(len(t)).astype(int), np.zeros(len(t)), np.ones(len(t))*0.5 + P_fuel, P_batt, P_ice, P_em, T_ice, T_em, w_e, trq_req, ef, I = np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)), np.zeros(len(t)) + + Gear[0] = int(gs_th(V[0])) + SOC[0] = 0.8 if hyd_mode=='CD' else 0.5 + gs_cnt = 10 + dd = 700 - for i in range(1, len(t)): + for i in tqdm(range(1, len(t))): if X[i-1] <= 0.3 * dd: V_n[i] = V_n[0] elif X[i-1] <= 1 * dd: @@ -42,14 +425,63 @@ def variable_speed_sim( else: V_n[i] = 0 - A[i] = accMFC(V[i-1], V_n[i], driver_style, ap_curve, dp_curve, will_acc_model, overshoot) + gs_cnt += 1 + if gs_cnt < 10: + Gear[i] = Gear[i-1] + else: + if int(gs_th(V[i-1]))>Gear[i-1]: + Gear[i] = Gear[i-1]+1 + gs_cnt = 0 + elif int(gs_th(V[i-1]))0: + A[i] = 0 V[i] = max(V[i-1] + A[i] * dt, 0) - X[i] = X[i-1] + (V[i] + V[i-1]) / 2 * dt + if hyd_mode=="CS": + FC[i], P_fuel[i], P_batt[i], P_ice[i], P_em[i], \ + T_ice[i], T_em[i], SOC[i], w_e[i], trq_req[i], ef[i], I[i] \ + = energy_consumption_cs_aecms( + car, A[i], V[i], V[i-1], Gear[i], \ + SOC_R[i-1], SOC[i-1]) + elif hyd_mode=="CD": + P_batt[i], SOC[i], T_em[i], w_e[i], I[i] = energy_consumption_cd(car, A[i], V[i], Gear[i], SOC[i-1]) + A[0] = A[1] + data_plot = [ + V, A, Gear, w_e*60/(2*np.pi), \ + P_batt*1e-3, SOC, T_em, I, \ + FC*1e3, P_ice*1e-3, T_ice, + ] + data_label = [ + 'v(m/s)', 'a(m/s2)', 'gear', 'w_b4_gear\n(rpm)', \ + 'P_batt(kW)', 'soc', 'T_em(Nm)', 'I(A)', \ + 'fc(ml)', 'P_ice(kW)', 'T_ice(Nm)', + ] + + fig, axs = plt.subplots(len(data_plot), sharex=True) + fig.suptitle('MFC hybrid energy consumption - '+hyd_mode) + + for i in range(len(data_plot)): + axs[i].plot(t, data_plot[i]) + if data_label[i]=='v(m/s)': + axs[i].plot(t, V_n, 'r--') + else: + axs[i].plot(t, np.zeros_like(t), 'g--') + axs[i].set_ylabel(data_label[i], rotation=0, labelpad=30) + if data_label[i]=='fc(ml)': + axs[i].text(200, 0.2, 'avg. fc (l/100km): '+str(round(np.sum(FC)/(X[-1]*1e-5), 1))) + axs[i].set_xlabel('time(s)') + axs[i].set_xlim(0, t[-1]) + plt.show() + return t, V_n, X, V, A @@ -102,7 +534,9 @@ def variable_speed_sim_dc( def accel_sim( - driver_style, + car, + driver_style, + gs_th, ap_curve, dp_curve, will_acc_model, @@ -123,13 +557,27 @@ def accel_sim( V_n = np.zeros(len(t)) V[0] = veh_max_speed + Gear = [int(gs_th(V[0]))] + gs_cnt = 0 + for i in range(1, len(t)): A[i] = accMFC(V[i-1], V_n[i], driver_style, ap_curve, dp_curve, will_acc_model, overshoot) - V[i] = max(V[i-1] + A[i] * dt, 0) - X[i] = X[i-1] + (V[i] + V[i-1]) / 2 * dt + + gs_cnt += 1 + if gs_cnt < 10: + Gear.append(Gear[-1]) + else: + if gs_th(V[i])>Gear[-1]: + Gear.append(Gear[-1]+1) + gs_cnt = 0 + if gs_th(V[i])gs_up_th[j]: + gs_up[i] += 1 + if veh_model_speed[i]>gs_down_th[j]: + gs_down[i] += 1 + gs_th = interp1d(veh_model_speed, gs_up, kind='nearest', fill_value="extrapolate") + for k in range(len(veh_model_speed)): acc_temp = [] for i in range(len(curves[0])): acc_temp.append(float(curves[0][i](veh_model_speed[k]))) - veh_model_acc.append(max(acc_temp)) + veh_model_acc_max.append(max(max(acc_temp), 0.5)) + veh_model_acc.append(float(curves[0][int(gs_th(veh_model_speed[k]))](veh_model_speed[k]))) veh_model_dec.append(min(dec_curve(veh_model_speed[k]), -1)) acc_curve = interpolate.CubicSpline( veh_model_speed, veh_model_acc) dec_curve = interpolate.CubicSpline( veh_model_speed, veh_model_dec) - return car_info, acc_curve, dec_curve, veh_model_speed, veh_model_acc, veh_model_dec, veh_max_speed + return car_info, acc_curve, dec_curve, veh_model_speed, veh_model_acc, veh_model_dec, veh_model_acc_max, veh_max_speed, gs_th def plt_exp_val_spd_accel( @@ -387,23 +852,26 @@ def plt_exp_val_spd_accel( plt.legend() plt.grid() plt.tight_layout() - plt.savefig(os.path.join(res_path, hyd_mode+'_MFC_curves.png')) + plt.savefig(os.path.join(res_path, 'ap_dp_curves_exp_val.png')) # plt.show() - # plt.close() + plt.close() def plt_accel_scenario( driver_style, + gs_th, acc_curve, dec_curve, veh_max_speed, car_info, + car, + hyd_mode, res_path ): - + """ t, V_n, X, V, A = [], [], [], [], [] for k in range(len(driver_style)): - t_, V_n_, X_, V_, A_ = accel_sim(driver_style[k], acc_curve, dec_curve, 'horizontal_b', veh_max_speed, 0, 200, 'acc') + t_, V_n_, X_, V_, A_ = accel_sim(car, driver_style[k], gs_th, acc_curve, dec_curve, 'horizontal_b', veh_max_speed, 0, 200, 'acc') t.append(t_) V_n.append(V_n_) X.append(X_) @@ -414,7 +882,7 @@ def plt_accel_scenario( t, V_n, X, V, A = [], [], [], [], [] for k in range(len(driver_style)): - t_, V_n_, X_, V_, A_ = accel_sim(driver_style[k], acc_curve, dec_curve, 'horizontal_b', veh_max_speed, 0, 100, 'dec') + t_, V_n_, X_, V_, A_ = accel_sim(car, driver_style[k], gs_th, acc_curve, dec_curve, 'horizontal_b', veh_max_speed, 0, 100, 'dec') t.append(t_) V_n.append(V_n_) X.append(X_) @@ -423,10 +891,10 @@ def plt_accel_scenario( temp_path = os.path.join(res_path, 'decel_sim') plot_traj(driver_style, t, V_n, X, V, A, temp_path) - + """ t, V_n, X, V, A = [], [], [], [], [] for k in range(len(driver_style)): - t_, V_n_, X_, V_, A_ = variable_speed_sim(driver_style[k], acc_curve, dec_curve, 'horizontal_b', veh_max_speed, 0) + t_, V_n_, X_, V_, A_ = variable_speed_sim(car, hyd_mode, driver_style[k], gs_th, acc_curve, dec_curve, 'horizontal_b', veh_max_speed, 0) t.append(t_) V_n.append(V_n_) X.append(X_) @@ -530,11 +998,12 @@ def get_sp_MFC(parameters, tp, sstart, sdes, rt, freq): mfc_dec_curve = parameters['mfc_dec_curve'] will_acc_model = parameters['will_acc_model'] overshoot = parameters['overshoot'] + gs_th = parameters['gs_th'] max_time = tp[-1] - tp[0] dt = 1 / freq n = int(np.ceil(max_time / dt) + 1) xp = np.arange(tp[0], tp[0] + n * dt, dt) - sp = sp_MFC(len(xp), sstart, sdes, rt, freq, mfc_acc_curve, mfc_dec_curve, will_acc_model, overshoot, driver_style) + sp = sp_MFC(len(xp), sstart, sdes, rt, freq, mfc_acc_curve, mfc_dec_curve, will_acc_model, overshoot, driver_style, gs_th) sp_mfc = np.interp(tp, xp, sp) return sp_mfc @@ -567,7 +1036,7 @@ def get_sp_IDM(parameters,tp,sstart,sdes, rt, freq, **kwargs): return sp -def sp_MFC(n, sstart, sdes, rt, freq, acc_p_curve, dec_p_curve, will_acc_model, overshoot, driver_style): +def sp_MFC(n, sstart, sdes, rt, freq, acc_p_curve, dec_p_curve, will_acc_model, overshoot, driver_style, gs_th): # rt = 0.5 steps = int((rt / (1 / freq)) - 1) if steps > 0: @@ -576,7 +1045,24 @@ def sp_MFC(n, sstart, sdes, rt, freq, acc_p_curve, dec_p_curve, will_acc_model, sp = [sstart] curr_speed = sstart + gs_cnt = 10 + gear_prev = int(gs_th(curr_speed)) + gear_curr = gear_prev + for i in range(1, n): + gs_cnt += 1 + if gs_cnt < 10: + gear_curr = gear_prev + else: + if int(gs_th(curr_speed))>gear_prev: + gear_curr = gear_prev+1 + gs_cnt = 0 + elif int(gs_th(curr_speed)) dist_travelled: @@ -863,9 +1356,22 @@ def follow_leader_mfc(instance, tp, lsp, sim_step, start_dist, start_speed): dhf_interp = np.interp(round_RT, [ceil_RT, floor_RT], [dhf[-ceil_RT], dhf[-floor_RT]]) shf_interp = np.interp(round_RT, [ceil_RT, floor_RT], [shf[-ceil_RT], shf[-floor_RT]]) + gs_cnt += 1 + if gs_cnt < 10: + gear_curr = gear_prev + else: + if int(gs_th(sf_prev))>gear_prev: + gear_curr = gear_prev+1 + gs_cnt = 0 + elif int(gs_th(sf_prev))0: + potential_acc = 0 + new_vel = fol_v + potential_acc * sim_step if new_vel < 0: # print("GippsMFC - new_vel negative!") diff --git a/c2art_env/sim_env/hybrid_mfc/reading_n_organizing.py b/c2art_env/sim_env/hybrid_mfc/reading_n_organizing.py index 318fe7d..6fbcff0 100644 --- a/c2art_env/sim_env/hybrid_mfc/reading_n_organizing.py +++ b/c2art_env/sim_env/hybrid_mfc/reading_n_organizing.py @@ -58,7 +58,7 @@ def get_vehicle_from_db(db_dict, car_id, **kwargs): elif my_car["Drive-Drive system"] in ['hybrid', 'plug-in hybrid']: powertrain = 'hybrid' - my_car_specs = vcc.veh_specs(my_car, powertrain, **kwargs) + my_car_specs = vcc.veh_specs(my_car, powertrain, lco=True, **kwargs) return my_car_specs diff --git a/c2art_env/sim_env/hybrid_mfc/utils.py b/c2art_env/sim_env/hybrid_mfc/utils.py index e380036..d93ef85 100644 --- a/c2art_env/sim_env/hybrid_mfc/utils.py +++ b/c2art_env/sim_env/hybrid_mfc/utils.py @@ -414,3 +414,90 @@ def setup_yaml_ordered(): yaml.add_representer(tuple, yaml.SafeDumper.represent_list) yaml.add_constructor(_MAPTAG, _construct_ordered_dict) + +def _clamp(value, limits): + lower, upper = limits + if value is None: + return None + elif (upper is not None) and (value > upper): + return upper + elif (lower is not None) and (value < lower): + return lower + return value + + +class PID(object): + """A simple PID controller.""" + # https://github.com/m-lundberg/simple-pid.git + + def __init__( + self, + Kp=1.0, + Ki=0.0, + Kd=0.0, + dt=0.1, + output_limits=(None, None), + ): + + self.Kp, self.Ki, self.Kd = Kp, Ki, Kd + + self._proportional = 0 + self._integral = 0 + self._derivative = 0 + self.dt=dt + + self._last_output = None + self._last_input = None + self._last_error = None + + self.output_limits = output_limits + self.reset() + + def __call__(self, setpoint_, input_): + """ + Update the PID controller. + """ + + # Compute error terms + error = setpoint_ - input_ + # d_input = input_ - (self._last_input if (self._last_input is not None) else input_) + d_error = error - (self._last_error if (self._last_error is not None) else error) + + # Compute the proportional term + self._proportional = self.Kp * error + + # Compute integral and derivative terms + self._integral += self.Ki * error * self.dt + self._integral = _clamp(self._integral, self.output_limits) # Avoid integral windup + + self._derivative = self.Kd * d_error / self.dt + + # Compute final output + output = self._proportional + self._integral + self._derivative + output = _clamp(output, self.output_limits) + + # Keep track of state + self._last_output = output + self._last_input = input_ + self._last_error = error + + return output + + + def reset(self): + """ + Reset the PID controller internals. + This sets each term to 0 as well as clearing the integral, the last output and the last + input (derivative calculation). + """ + self._proportional = 0 + self._integral = 0 + self._derivative = 0 + + self._integral = _clamp(self._integral, self.output_limits) + + self._last_output = None + self._last_input = None + self._last_error = None + + diff --git a/c2art_env/sim_env/hybrid_mfc/vehicle_specs_class.py b/c2art_env/sim_env/hybrid_mfc/vehicle_specs_class.py index 6d4be4d..344299a 100644 --- a/c2art_env/sim_env/hybrid_mfc/vehicle_specs_class.py +++ b/c2art_env/sim_env/hybrid_mfc/vehicle_specs_class.py @@ -189,6 +189,8 @@ def __init__(self, my_car, powertrain, **kwargs): self.fuel_turbo = my_car["Fuel Engine-Turbo"] self.fuel_eng_capacity = float(my_car["Fuel Engine-Capacity"]) self.gearbox_type = str(my_car["General Specifications-Transmission"]) + self.battery_capacity = float(my_car["Electric Engine-Battery capacity"]) # kWh + self.battery_voltage = float(my_car["Electric Engine-Battery voltage"]) # V class hardcoded_params(object): diff --git a/c2art_env/sim_env/platooning_mpc/__init__.py b/c2art_env/sim_env/platooning_mpc/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/c2art_env/sim_env/platooning_mpc/setup_utils.py b/c2art_env/sim_env/platooning_mpc/setup_utils.py new file mode 100644 index 0000000..16a6163 --- /dev/null +++ b/c2art_env/sim_env/platooning_mpc/setup_utils.py @@ -0,0 +1,690 @@ +import os, math, time, datetime +import matplotlib.pyplot as plt +import pandas as pd +import numpy as np +from numba import njit, types +from numba.typed import Dict +from scipy.optimize import minimize, NonlinearConstraint, Bounds +import networkx as nx +from c2art_env.veh_model.mfc.mfc_constraints import mfc_curves +from c2art_env.veh_model.longitudinal_dyn_veh import nonlinear_lon_dyn, linear_lon_dyn, none_lon_dyn +from c2art_env import utils + + +def parameter_init(follIDs): + + mfc_veh_load = 250 + mfc_rolling_coef = 0.009 + mfc_aero_coef = { + '33333': 0.27, + '1458': 0.35, + '8422': 0.27, + '50010': 0.30 + } + mfc_res_coef_1 = 0.84 + mfc_res_coef_2 = -71.735 + mfc_res_coef_3 = 2.7609 + mfc_ppar_0 = 0.0045 + mfc_ppar_1 = -0.1710 + mfc_ppar_2 = -1.8835 + mfc_mh_base = 0.75 + + ctrl_a_max = 3.0 # Acceleration bound (m/s2) + ctrl_a_min = -3.0 # Deceleration bound (m/s2) + a_max, a_min, f_0, f_1, f_2, veh_mass, phi, tau_a, mfc_array, mfc_slice = \ + [], [], [], [], [], [], [], [], [], [0] + + for i in range(len(follIDs)): + mfc_model = mfc_curves( + follIDs[i], + mfc_veh_load, + mfc_rolling_coef, + mfc_aero_coef[str(follIDs[i])], + mfc_res_coef_1, + mfc_res_coef_2, + mfc_res_coef_3, + mfc_ppar_0, + mfc_ppar_1, + mfc_ppar_2, + mfc_mh_base) + + total_mass = mfc_model['car_mass'] + mfc_veh_load + + f_0.append(mfc_model['mfc_f_0']) + f_1.append(mfc_model['mfc_f_1']) + f_2.append(mfc_model['mfc_f_2']) + veh_mass.append(total_mass) + tau_a.append(0.5 +(total_mass - 1000)/1000 * 0.3) # Time lag + phi.append(mfc_model['car_phi']) + a_max.append(ctrl_a_max) + a_min.append(ctrl_a_min) + + if i == 0: + mfc_array = np.array([ + mfc_model['mfc_speed'], + mfc_model['mfc_acc'], + mfc_model['mfc_dec'] + ]) + else: + mfc_array = np.append(mfc_array, + np.array([ + mfc_model['mfc_speed'], + mfc_model['mfc_acc'], + mfc_model['mfc_dec'] + ]), axis=1) + mfc_slice.append(len(mfc_array[0])) + + return np.array(a_max), np.array(a_min), np.array(f_0), np.array(f_1), np.array(f_2), \ + np.array(phi), np.array(tau_a), np.array(veh_mass), mfc_array, np.array(mfc_slice) + + +@njit +def leader_init( + numStep, + timeStep + ): + p0 = np.zeros(numStep) + v0 = np.zeros(numStep) + a0 = np.zeros(numStep) + + # Transient process of leader, which is given in advance + p0[0] = 0 + v0[0] = 20 + a0[int(1/timeStep):int(2/timeStep)] = 2 + + ### Integration I ### + # for i in range(1, numStep): + # v0[i] = v0[i-1] + a0[i] * timeStep + # p0[i] = p0[i-1] + v0[i] * timeStep + + ### Integration II ### + for i in range(numStep-1): + state_next = utils.motion_integ( + p0[i], + v0[i], + a0[i+1], + timeStep + ) + p0[i+1], v0[i+1], a0[i+1]= state_next[0], state_next[1], state_next[2] + + return p0, v0, a0 + + +@njit +def variable_init( + numFoll, + numStep, + timeStep, + Np, # Predictive horizon + d, + f_0, + f_1, + f_2, + phi, + tau_a, + veh_mass, + mfc_array, + mfc_slice + ): + + sin_theta = 0 + # Initial Virables + Postion = np.zeros((numFoll, numStep)) # Postion of following vehicles + Velocity = np.zeros((numFoll, numStep)) # Velocity of following vehicles + Acceleration = np.zeros((numFoll, numStep)) # Braking or tracking acceleration of following vehicles + U = np.zeros((numFoll, numStep)) # Desired braking or tracking acceleration of following vehicles + + Cost = np.zeros((numFoll, numStep)) # Cost function + Exitflg = np.zeros((numFoll, numStep)) # Stop flag - solvers + + # Leading vehicle + p0, v0, a0 = leader_init(numStep, timeStep) + + # Zero initial error for the followers + for i in range(numFoll): + Postion[i][0] = p0[0] - (i+1)*d + Velocity[i][0] = v0[0] + Acceleration[i][0] = a0[0] + + # Distributed MPC assumed state + Pa = np.zeros((numFoll, Np+1)) # Assumed postion of each vehicle + Va = np.zeros((numFoll, Np+1)) # Assumed velocity of each vehicle + Aa = np.zeros((numFoll, Np+1)) # Assumed acceleration of each vehicle + ua = np.zeros((numFoll, Np)) # Assumed Braking or Tracking Torque input of each vehicle + + Pa_next = np.zeros((numFoll, Np+1)) # 1(0): Assumed postion of each vehicle at the next time step + Va_next = np.zeros((numFoll, Np+1)) # Assumed velocity of each vehicle at the next time step + Aa_next = np.zeros((numFoll, Np+1)) + ua_next = np.zeros((numFoll, Np+1)) # Assumed Braking or Tracking acceleration of each vehicle at the next time step + + # Initialzie the assumed state for the first computation: constant speed + for i in range(numFoll): + ua[i] = (f_0[i] * (1 - sin_theta**2)**0.5 + f_1[i] * Velocity[i][0] + f_2[i] * Velocity[i][0]**2) / veh_mass[i] + 9.80665 * sin_theta + Pa[i][0] = Postion[i][0] # The first point should be interpreted as k = 0 (current state) + Va[i][0] = Velocity[i][0] + Aa[i][0] = Acceleration[i][0] + for j in range(Np): + Pa[i][j+1], Va[i][j+1], Aa[i][j+1] = vehicle_dynamic( + ua[i][j], + Pa[i][j], + Va[i][j], + Aa[i][j], + timeStep, + sin_theta, + f_0[i], + f_1[i], + f_2[i], + phi[i], + tau_a[i], + veh_mass[i], + mfc_array[:, mfc_slice[i]:mfc_slice[i+1]]) + # For debugging + # Terminal state + Pend = np.zeros((numFoll, numStep)) + Vend = np.zeros((numFoll, numStep)) + Aend = np.zeros((numFoll, numStep)) + + return Postion, Velocity, Acceleration, U, p0, v0, a0, Pa, Va, Aa, ua, \ + Pa_next, Va_next, Aa_next, ua_next, Pend, Vend, Aend, Cost, Exitflg + + +@njit +def vehicle_dynamic( + u, + Position, + Velocity, + Acceleration, + timeStep, + sin_theta, + f_0, + f_1, + f_2, + phi, + tau_a, + veh_mass, + mfc_curve + ): + + accel_next = nonlinear_lon_dyn( + Velocity, + Acceleration, + u, + sin_theta, + timeStep, + phi, + tau_a, + veh_mass, + f_0, + f_1, + f_2) + + # accel_next = linear_lon_dyn( + # Velocity, + # Acceleration, + # u, + # timeStep, + # tau_a) + + # accel_next = none_lon_dyn( + # Velocity, + # u) + + mfc_a_max = utils.interp_binary(mfc_curve[0], mfc_curve[1], Velocity) + mfc_a_min = utils.interp_binary(mfc_curve[0], mfc_curve[2], Velocity) + + accel_next = utils.clip_min_max(accel_next, mfc_a_min, mfc_a_max) + + ### Integration I ### + # v_next = Velocity + accel_next * timeStep + # p_next = Position + v_next * timeStep + # state_next = np.array([ + # p_next, + # v_next, + # accel_next + # ]) + + ### Integration II ### + state_next = utils.motion_integ( + Position, + Velocity, + accel_next, + timeStep + ) + + return state_next[0], state_next[1], state_next[2] + + +def communication_init( + numFoll, + TopoType + ): + G = nx.DiGraph() + ############### + # Vertice set # + ############### + G.add_nodes_from(range(1, numFoll+1)) + ############ + # Edge set # + ############ + if TopoType in ['PF', 'PLF']: + for i in G.nodes: + if i+1 in G.nodes: + G.add_edge(i, i+1) + elif TopoType in ['BD', 'BDL']: + for i in G.nodes: + if i-1 in G.nodes: + G.add_edge(i, i-1) + if i+1 in G.nodes: + G.add_edge(i, i+1) + elif TopoType in ['TPF', 'TPLF']: + for i in G.nodes: + if i+1 in G.nodes: + G.add_edge(i, i+1) + if i+2 in G.nodes: + G.add_edge(i, i+2) + matA = nx.adjacency_matrix(G).todense().T # Adjacency matrix + matI = nx.incidence_matrix(G, oriented=True).todense() # Incidence matrix + matDin = np.diag([G.in_degree(i) for i in G.nodes]) + matDout = np.diag([G.out_degree(i) for i in G.nodes]) + matL = matDin - matA # Laplace matrix + ### In-neighbour set ### + setPred = [list(G.predecessors(i)) for i in G.nodes] + ### Out-neighbour set ### + setSucc = [list(G.successors(i)) for i in G.nodes] + ####################### + # Pinning information # + ####################### + if TopoType in ['PF', 'BD']: + matP = np.zeros((numFoll, numFoll)) + matP[0, 0] = 1 + elif TopoType in ['TPF']: + matP = np.zeros((numFoll, numFoll)) + matP[0, 0] = 1 + matP[1, 1] = 1 + elif TopoType in ['PLF', 'BDL', 'TPLF']: + matP = np.eye(numFoll) + ### Pinning in-neighbour set ### + setIN = [] + for i in range(len(G.nodes)): + if matP[i, i] == 1: + setIN.append([0]+setPred[i]) + elif matP[i, i] == 0: + setIN.append(setPred[i]) + + # print(matA) + # print(matI) + # print(matDin) + # print(matDout) + # print(matL) + # print(setPred) + # print(setSucc) + # print(matP) + # print(setIN) + + # nx.draw(G, with_labels = True) + # plt.show() + return matA, matP + + +@njit +def mpc_func( + Np, + timeStep, + sin_theta, + X0, + u, + vehType, + mfc_curve + ): + + f_0 = vehType[0] + f_1 = vehType[1] + f_2 = vehType[2] + phi = vehType[3] + tau_a = vehType[4] + veh_mass = vehType[5] + + + Pp = np.zeros(Np+1) # Predictive position (m) + Vp = np.zeros(Np+1) # Predictive velocity (m/s) + Ap = np.zeros(Np+1) # Predictive acceleration (m/s2) + + Pp[0], Vp[0], Ap[0] = X0[0], X0[1], X0[2] + + for i in range(Np): + Pp[i+1], Vp[i+1], Ap[i+1] = vehicle_dynamic( + u[i], + Pp[i], + Vp[i], + Ap[i], + timeStep, + sin_theta, + f_0, + f_1, + f_2, + phi, + tau_a, + veh_mass, + mfc_curve) + + Xp = np.vstack((Pp, Vp, Ap)) # Predictive state + + return Xp + + +@njit +def nonlcon_func(u, *args): + # args = args[0] + + Np = args[0] + timeStep = args[1] + sin_theta = args[2] + X0 = args[3] + vehType = args[4] + mfc_curve = args[5] + Pnp = args[6] + Vnp = args[7] + Anp = args[8] + + Xp = mpc_func( + Np, + timeStep, + sin_theta, + X0, + u, + vehType, + mfc_curve + ) + Pp = Xp[0] + Vp = Xp[1] + Ap = Xp[2] + + Ceq = np.array([ + Pp[Np]-Pnp, + Vp[Np]-Vnp, + Ap[Np]-Anp + ]) + + return Ceq + + +@njit +def cost_func(u, *args): + # args = args[0] + + Np = args[0] + timeStep = args[1] + sin_theta = args[2] + X0 = args[3] + vehType = args[4] + mfc_curve = args[5] + Q = args[6] + Xdes = args[7].transpose() + R = args[8] + F = args[9] + Xa = args[10].transpose() + G = args[11] + Xn = args[12].transpose() + numXn = args[13] + + f_0 = vehType[0] + f_1 = vehType[1] + f_2 = vehType[2] + phi = vehType[3] + tau_a = vehType[4] + veh_mass = vehType[5] + + dim = Xa.shape[1] + + Xp = mpc_func( + Np, + timeStep, + sin_theta, + X0, + u, + vehType, + mfc_curve + ) + + Vp = Xp[1] + Xp = Xp[:dim].transpose() + + Udes = (f_0 * (1 - sin_theta**2)**0.5 + f_1 * Vp + f_2 * Vp**2) / veh_mass + 9.80665 * sin_theta + + cost = np.zeros(Np) # Cost array + + for i in range(Np): + cost_des = (Xp[i]-Xdes[i]) @ Q @ (Xp[i]-Xdes[i]) + cost_u = (u[i]-Udes[i]) * R * (u[i]-Udes[i]) + cost_self = (Xp[i]-Xa[i]) @ F @ (Xp[i]-Xa[i]) + cost_nbr = 0 + for j in range(numXn): + cost_nbr += (Xp[i]-Xn[i, dim*j:dim*j+dim]) @ G @ (Xp[i]-Xn[i, dim*j:dim*j+dim]) + + cost[i] = cost_des + cost_u + cost_self + cost_nbr + + cost_sum = np.sum(cost) + + return cost_sum + + +def mpc_simulation( + d, + Np, + numVeh, + numStep, + timeStep, + # Communication topology + matA, + matP, + # Vehicle type + a_max, + a_min, + f_0, + f_1, + f_2, + veh_mass, + phi, + tau_a, + mfc_array, + mfc_slice + ): + + numFoll = numVeh-1 + + Postion, Velocity, Acceleration, U, p0, v0, a0, Pa, Va, Aa, ua, \ + Pa_next, Va_next, Aa_next, ua_next, Pend, Vend, Aend, Cost, Exitflg = variable_init( \ + numFoll, numStep, timeStep, Np, d, f_0, f_1, f_2, \ + phi, tau_a, veh_mass, mfc_array, mfc_slice) + + vehTypeCol = np.vstack(( + f_0, # 0 + f_1, # 1 + f_2, # 2 + phi, # 3 + tau_a, # 4 + veh_mass,# 5 + )).transpose() + + for i in range(1, numStep-Np): + print('Steps i='+str(i)) + for j in range(numFoll): # IDX of followers (0, 1, 2, 3 ...) + start_time = time.time() + + vehType = vehTypeCol[j] + mfc_curve = mfc_array[:, mfc_slice[j]:mfc_slice[j+1]] + + X0 = np.array([ + Postion[j][i-1], + Velocity[j][i-1], + Acceleration[j][i-1], + ]) + + # Input-deviation + R = 0.1 + + # Self-deviation + Xa = np.vstack((Pa[j], Va[j], Aa[j])) + F = np.diag([5.0, 2.5, 1.0]) * (np.sum(matA[:,j])+1)**2 + + # Leader-deviation + if matP[j, j] == 1: + numXdes = 1 + Pd = p0[i-1:i+Np] - (j+1)*d + Vd = v0[i-1:i+Np] + Ad = a0[i-1:i+Np] + Xdes = np.vstack((Pd, Vd, Ad)) + Q = np.diag([5.0, 2.5, 1.0]) + else: + numXdes = 0 + Xdes = np.zeros((3, Np+1)) + Q = np.diag([0.0, 0.0, 0.0]) + + # Neighbor-deviation + numXn = np.sum(matA[j,:]) + if numXn == 0: + Xn = np.zeros((3, Np+1)) + G = np.diag([0.0, 0.0, 0.0]) + else: + Xn = np.zeros((0, Np+1)) + for k in range(numFoll): + if matA[j,k] == 1: + Xn = np.vstack((Xn, Pa[k] - (j-k)*d, Va[k], Aa[k])) + G = np.diag([5.0, 2.5, 1.0]) + + lb = a_min[j] * np.ones(Np) + ub = a_max[j] * np.ones(Np) + u0 = ua[j] + + Pnp = Xdes[0][-1] + Vnp = Xdes[1][-1] + Anp = Xdes[2][-1] + + for k in range(numXn): + Pnp += Xn[3*k][-1] + Vnp += Xn[3*k+1][-1] + Anp += Xn[3*k+2][-1] + + Pnp = Pnp/(numXdes+numXn) + Vnp = Vnp/(numXdes+numXn) + Anp = Anp/(numXdes+numXn) + + Pend[j][i] = Pnp + Vend[j][i] = Vnp + Aend[j][i] = Anp + + sin_theta = 0 + args_cost=( + Np, # 0 + timeStep, # 1 + sin_theta, # 2 + X0, # 3 + vehType, # 4 + mfc_curve, # 5 + Q, # 6 + Xdes, # 7 + R, # 8 + F, # 9 + Xa, # 10 + G, # 11 + Xn, # 12 + numXn # 13 + ) + args_cons=( + Np, # 0 + timeStep, # 1 + sin_theta, # 2 + X0, # 3 + vehType, # 4 + mfc_curve, # 5 + Pnp, # 6 + Vnp, # 7 + Anp # 8 + ) + # bnds = tuple([(lb[k], ub[k]) for k in range(len(lb))]) + bnds = Bounds(lb, ub) + cons = ({'type': 'eq', + 'fun': nonlcon_func, + 'args': args_cons + }) + res = minimize(cost_func, u0, bounds=bnds, args=args_cost, method='SLSQP', constraints=cons, \ + tol = 1e-5, + options={ + 'ftol': 1e-5, + # 'eps': 1e-4, + 'maxiter': 2000, + # 'disp': True + } + ) + + Cost[j][i] = res.fun + u = res.x + Exitflg[j][i] = res.status + nit = res.nit + + U[j][i] = u[0] + + Postion[j][i], Velocity[j][i], Acceleration[j][i] = vehicle_dynamic( + U[j][i], + Postion[j][i-1], + Velocity[j][i-1], + Acceleration[j][i-1], + timeStep, + sin_theta, + f_0[j], + f_1[j], + f_2[j], + phi[j], + tau_a[j], + veh_mass[j], + mfc_curve) + + Temp = np.zeros((3, Np+1)) + Temp[0][0], Temp[1][0], Temp[2][0] = Postion[j][i], Velocity[j][i], Acceleration[j][i] + ua[j][:Np-1] = u[1:] + + for k in range(Np-1): + Temp[0][k+1], Temp[1][k+1], Temp[2][k+1] = vehicle_dynamic( + ua[j][k], + Temp[0][k], + Temp[1][k], + Temp[2][k], + timeStep, + sin_theta, + f_0[j], + f_1[j], + f_2[j], + phi[j], + tau_a[j], + veh_mass[j], + mfc_curve) + + ua[j][Np-1] = (f_0[j] * (1 - sin_theta**2)**0.5 + f_1[j] * Temp[1][Np-1] + f_2[j] * Temp[1][Np-1]**2) / veh_mass[j] + 9.80665 * sin_theta + + Temp[0][Np], Temp[1][Np], Temp[2][Np] = vehicle_dynamic( + ua[j][Np-1], + Temp[0][Np-1], + Temp[1][Np-1], + Temp[2][Np-1], + timeStep, + sin_theta, + f_0[j], + f_1[j], + f_2[j], + phi[j], + tau_a[j], + veh_mass[j], + mfc_curve) + + Pa_next[j] = Temp[0] + Va_next[j] = Temp[1] + Aa_next[j] = Temp[2] + + print(time.time() - start_time) + + Pa = Pa_next + Va = Va_next + Aa = Aa_next + + return U, Postion, Velocity, Acceleration, p0, v0, a0, Pend, Vend, Aend + diff --git a/c2art_env/veh_model/mfc/car_database/2019_07_03_car_db.csv b/c2art_env/veh_model/mfc/car_database/2019_07_03_car_db.csv index 2603371..5020ffc 100644 --- a/c2art_env/veh_model/mfc/car_database/2019_07_03_car_db.csv +++ b/c2art_env/veh_model/mfc/car_database/2019_07_03_car_db.csv @@ -6,5 +6,5 @@ 50010,https://www.cars-data.com/en/mercedes-a-250-4matic-sport-motorsport-edition-specs/73863,Mercedes A 250 4MATIC Sport Motorsport Edition,,56226,hatchback,automatic,5,2015,2017,front+rear,fuel engine,petrol,160,350,4,4,1991,9.8,160,5500,350,1200,injection,dohc,yes,regular,56,,,,,,,,,,240,6.3,8.4,5.6,6.6,156,F,4.6,3800,1405,2030,100,4.299,1.78,1.433,2.699,,yes,yes,no,no,yes,yes,yes,yes,no,no,381,817,yes,yes,5,92,323.55,314.1262136,[3.86- 2.43- 2.67- 1.05- 0.78- 1.05- 0.84],1405,1480,1505,1470,2.55074 26573,http://www.cars-data.com/en/volkswagen-golf-2.0-tdi-150hp-highline-specs/59579,Volkswagen Golf 2.0 TDI 150hp Highline 2012 - 2016,,33690,hatchback,automatic,5,2012,available,front,fuel engine,diesel,110,320,4,4,1968,,110,3500,320,1750,common rail,dohc,yes,particle filter,50,,,,,,,,,,216,8.6,5.2,4,4.4,117,D,3.04,2300,1275,1880,75,4.255,1.799,1.452,2.637,,yes,334,,,,yes,yes,yes,yes,yes,,,,yes,3,95.5,294.875,307.9126214,[3.46- 2.05- 1.34- 0.99- 0.82- 0.76],1241.25,1316.25,1341.25,1360,2.612148 26687,https://www.cars-data.com/en/hyundai-ioniq-electric-comfort-specs/76078,Hyundai Ioniq Electric Comfort,,31500,hatchback,,5,2016,available,front,electric engine,,88,,,,,,,,,,,,no,no,,synchronous motor ,88,295,1,lithium-ion,28,,280,11.5,165,9.9,,,,,A,1,,1420,1880,,4.47,1.82,1.45,2.7,0.14,yes,yes,yes,no,yes,yes,yes,yes,yes,yes,yes,no,yes,yes,5,,315.95,306.7475728,[7.412],1420,1495,1520,1470,2.639 -33333,,Tesla Model 3,,,sedan,,,,,rear,electric engine,,,,,,,,,,,,,,,,,,211,330,1,lithium-ion,,,,,250,,,,,,,1,,1980,,,4.694,1.85,1.443,2.875,,,,,,,,,,,,,,,,,,336.5,,[9.0],1980,,,2050, +33333,,Tesla Model 3,,,sedan,,,,,rear,electric engine,,,,,,,,,,,,,,,,,,211,330,1,lithium-ion,,,,,250,5.6,,,,,,1,,1980,,,4.694,1.85,1.443,2.875,,,,,,,,,,,,,,,,,,336.5,,[9.0],1980,,,2050, 3356,https://www.cars-data.com/en/skoda-octavia-2.0-tsi-rs-230-specs/69323,Skoda Octavia 2.0 TSI RS 230,C,43890,hatchback,automatic,5,2016,available,front,fuel engine,petrol,169,350,4,4,1984,9.6,169,4700,350,1500,injection,ohc,yes,regular,50,,,,,,,,,,249,6.8,8.1,5.4,6.4,149,D,3.44,2200,1365,1932,75,4.685,1.814,1.449,2.68,0.128,yes,yes,,no,yes,yes,yes,yes,yes,yes,no,yes,no,yes,5,92.8,320.05,310.7281553,[2.92- 1.79- 1.14- 0.78- 0.80- 0.64],1365,1440,1465,1470,2.628486 diff --git a/environment.yml b/environment.yml new file mode 100644 index 0000000..f136061 --- /dev/null +++ b/environment.yml @@ -0,0 +1,66 @@ +name: jrc +channels: + - defaults +dependencies: + - bzip2=1.0.8=h2bbff1b_5 + - ca-certificates=2024.3.11=haa95532_0 + - libffi=3.4.4=hd77b12b_0 + - openssl=3.0.13=h2bbff1b_0 + - pip=23.3.1=py310haa95532_0 + - python=3.10.14=he1021f5_0 + - setuptools=68.2.2=py310haa95532_0 + - sqlite=3.41.2=h2bbff1b_0 + - tk=8.6.12=h2bbff1b_0 + - vc=14.2=h21ff451_1 + - vs2015_runtime=14.27.29016=h5e58377_2 + - wheel=0.41.2=py310haa95532_0 + - xz=5.4.6=h8cc25b3_0 + - zlib=1.2.13=h8cc25b3_0 + - pip: + - asteval==0.9.32 + - asttokens==2.4.1 + - colorama==0.4.6 + - contourpy==1.2.1 + - cycler==0.12.1 + - decorator==5.1.1 + - dill==0.3.8 + - et-xmlfile==1.1.0 + - exceptiongroup==1.2.0 + - executing==2.0.1 + - fonttools==4.51.0 + - func-timeout==4.3.5 + - future==1.0.0 + - ipython==8.23.0 + - jedi==0.19.1 + - kiwisolver==1.4.5 + - llvmlite==0.42.0 + - lmfit==1.3.0 + - matplotlib==3.8.4 + - matplotlib-inline==0.1.6 + - networkx==3.3 + - numba==0.59.1 + - numpy==1.25.0 + - openpyxl==3.1.2 + - packaging==24.0 + - pandas==2.2.2 + - parso==0.8.4 + - pillow==10.3.0 + - plotly==5.20.0 + - prompt-toolkit==3.0.43 + - pure-eval==0.2.2 + - pygments==2.17.2 + - pyparsing==3.1.2 + - python-dateutil==2.9.0.post0 + - pytz==2024.1 + - pyyaml==6.0.1 + - scipy==1.13.0 + - six==1.16.0 + - stack-data==0.6.3 + - tenacity==8.2.3 + - tqdm==4.66.2 + - traitlets==5.14.2 + - typing-extensions==4.11.0 + - tzdata==2024.1 + - uncertainties==3.1.7 + - wcwidth==0.2.13 +prefix: C:\Users\leiti\miniconda3\envs\jrc