├── AutonomousCarTruckCurveRoadControl.py ├── ImageFolder ├── BlueCar.png ├── BlueTruck.png ├── GreenCar.png ├── GreenCar_HV.png ├── GreenTruck.png ├── GreyCar.png ├── GreyTruck.png ├── LeftTurn.png ├── LeftTurn2.png ├── LeftTurn3.png ├── PinkCar.png ├── PurpleCar.png ├── RedCar.png ├── RedTruck.png ├── RightTurn3.png ├── Road.png ├── YellowCar.png ├── YellowTruck.png ├── circuit - Copy.png ├── circuit.jpg ├── circuit.png ├── circuit1.png ├── circuit2.jpg ├── circuit2.png ├── circuit3.png ├── circuit_PS.png ├── largedash.png ├── straight.jpg └── straight.png ├── MaxEntDeepIRL_SingleStep.py ├── README.txt ├── save_optimal_policy.txt └── video.py /AutonomousCarTruckCurveRoadControl.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue May 30 14:25:51 2017 4 | @author: CYOU7 5 | 6 | """ 7 | 8 | from video import make_video 9 | import pygame, sys 10 | from pygame.locals import* 11 | from math import atan 12 | import random as rd 13 | import cvxopt as co 14 | import cvxpy as cp 15 | import math as mh 16 | import matplotlib.pyplot as plt 17 | from numpy import arctan2 as atan2 18 | import numpy as np 19 | 20 | np.random.seed(0) # make it reproducible 21 | Nr_action = 5 22 | ActionSet = ['stay','acc','brk','left','right'] 23 | ColorSet = ['BlueCar','RedCar','PinkCar','YellowCar','GreyCar','PurpleCar','BlueTruck','RedTruck','YellowTruck','GreenTruck','GreyTruck'] 24 | ColorSet_HV = ['GreenCar_HV'] 25 | 26 | #Pr_actions = np.random.rand(Nr_action) 27 | #Pr_actions = Pr_actions/sum(Pr_actions) # normalize action probabilities 28 | #Pr_actions.sort() # increasing order 29 | Pr_actions = np.array([0.05, 0.05, 0.05, 0.3, 0.55]) 30 | 31 | NrLanes = 5 # road of 5 lanes, 0~4 from left to right 32 | dt = 0.01 33 | extra_dis = 5 # for lane keeping 34 | g = 9.81 35 | Resize = 130/4.5 36 | Period = int(1.0/dt) # number of steps to add EVs, every 1 sec 37 | 38 | maintain_Time = 1.0 39 | 40 | display_width = 800 #1000 41 | display_height = 1200 #1000 42 | width = 110 43 | length = 175 44 | car_width = 66 45 | car_length = 130 46 | road_width = 1048 47 | road_length = 2200 48 | 49 | WidthSet=[car_width]*6+[74,68,72,75,70] 50 | LengthSet=[car_length]*6+[197,140,197,172,185] 51 | 52 | road_margin = road_length-display_height 53 | 54 | circuit_edge = 3016 55 | circuit_edge = 3008 56 | 57 | R3 = circuit_edge-road_width/2 58 | R1 = R3 - 2*width 59 | R2 = R3 - width 60 | R4 = R3 + width 61 | R5 = R3 + 2*width 62 | 63 | #============================================================================== 64 | # from state vector to vehicle array 65 | def state2matrix(state_aux): # state_aux: 1~3 for HV + binary[1-8] for EV 66 | VehicleArray =np.zeros((3,3)) 67 | 68 | VehicleArray[0,:] = state_aux[1:4] 69 | VehicleArray[2,:] = state_aux[6:9] 70 | if state_aux[0] == 1: 71 | VehicleArray[1,0] = 2 # distinguish from binary value, indicate HV 72 | VehicleArray[1,1:3] = state_aux[4:6] 73 | elif state_aux[0] == 3: 74 | VehicleArray[1,2] = 2 75 | VehicleArray[1,0:2] = state_aux[4:6] 76 | elif state_aux[0] == 2: 77 | VehicleArray[1,1] = 2 78 | VehicleArray[1,0] = state_aux[4] 79 | VehicleArray[1,2] = state_aux[5] 80 | return VehicleArray 81 | 82 | #============================================================================== 83 | # from vehicle array to state vector 84 | def matrix2state(VehicleArray_aux): 85 | 86 | state = np.zeros(9) 87 | state[1:4] = VehicleArray_aux[0,:] 88 | state[6:9] = VehicleArray_aux[2,:] 89 | 90 | indices = np.asarray(np.where(VehicleArray_aux == 2 )).T # get position of HV 91 | 92 | if indices[0][1] == 0: 93 | state[0] = 1 94 | state[4:6] = VehicleArray_aux[1,1:3] 95 | elif indices[0][1] == 1: 96 | state[0] = 2 97 | state[4] = VehicleArray_aux[1,0] 98 | state[5] = VehicleArray_aux[1,2] 99 | elif indices[0][1] == 2: 100 | state[0] = 3 101 | state[4:6] = VehicleArray_aux[1,0:2] 102 | 103 | return state 104 | #============================================================================== 105 | # safe actions of HV 106 | def Safe_actions_HV(state_aux): # HV_lane=0 <=> state_aux[0] = 1, HV_lane=right lane <=> state_aux[0] = 3 107 | 108 | VehicleArray_aux = state2matrix(state_aux) 109 | if state_aux[0] == 1: # fill 1 to occupy left boundary 110 | aux0 = np.concatenate(([1],VehicleArray_aux[0,:],[0])) 111 | aux1 = np.concatenate(([1],VehicleArray_aux[1,:],[0])) 112 | aux2 = np.concatenate(([1],VehicleArray_aux[2,:],[0])) 113 | VehicleArray_aux2 = np.array([[1,0,0,0,0],aux0,aux1,aux2,[1,0,0,0,0]]) #extended to 5x5 with zeros & ones, left boundary considered 114 | pos_HV = np.array([1,0]) 115 | elif state_aux[0] == 3: 116 | aux0 = np.concatenate(([0],VehicleArray_aux[0,:],[1])) 117 | aux1 = np.concatenate(([0],VehicleArray_aux[1,:],[1])) 118 | aux2 = np.concatenate(([0],VehicleArray_aux[2,:],[1])) 119 | VehicleArray_aux2 = np.array([[0,0,0,0,1],aux0,aux1,aux2,[0,0,0,0,1]]) #extended to 5x5 with zeros & ones, right boundary considered 120 | pos_HV = np.array([1,2]) 121 | elif state_aux[0] == 2: 122 | aux0 = np.concatenate(([0],VehicleArray_aux[0,:],[0])) 123 | aux1 = np.concatenate(([0],VehicleArray_aux[1,:],[0])) 124 | aux2 = np.concatenate(([0],VehicleArray_aux[2,:],[0])) 125 | VehicleArray_aux2 = np.array([[0,0,0,0,0],aux0,aux1,aux2,[0,0,0,0,0]]) #no boundary cases 126 | pos_HV = np.array([1,1]) 127 | pos_aux = pos_HV+[1,1] # new position in 5x5 array (5x5 not necessary, just make it uniform with context) 128 | 129 | pos_aux_acc = pos_aux + [-1,0] 130 | if VehicleArray_aux2[pos_aux_acc[0],pos_aux_acc[1]] == 0: 131 | acc = 1 132 | else: 133 | acc = 0 134 | pos_aux_brk = pos_aux + [1,0] 135 | if VehicleArray_aux2[pos_aux_brk[0],pos_aux_brk[1]] == 0: 136 | brk = 1 137 | else: 138 | brk = 0 139 | pos_aux_left = pos_aux + [0,-1] 140 | if VehicleArray_aux2[pos_aux_left[0],pos_aux_left[1]] == 0: 141 | left = 1 142 | else: 143 | left = 0 144 | 145 | pos_aux_right = pos_aux + [0,1] 146 | if VehicleArray_aux2[pos_aux_right[0],pos_aux_right[1]] == 0: 147 | right = 1 148 | else: 149 | right = 0 150 | 151 | return acc, brk, left, right 152 | #============================================================================== 153 | # available actions of each EV 154 | def Available_actions_EV(state_matrix): # fake the state matrix for each EV (in center) 155 | 156 | pos_aux = np.array([1,1]) 157 | 158 | pos_aux_acc = pos_aux + [-1,0] 159 | if state_matrix[pos_aux_acc[0],pos_aux_acc[1]] == 0: 160 | acc = 1 161 | else: 162 | acc = 0 163 | pos_aux_brk = pos_aux + [1,0] 164 | if state_matrix[pos_aux_brk[0],pos_aux_brk[1]] == 0: 165 | brk = 1 166 | else: 167 | brk = 0 168 | pos_aux_left = pos_aux + [0,-1] 169 | if state_matrix[pos_aux_left[0],pos_aux_left[1]] == 0: 170 | left = 1 171 | else: 172 | left = 0 173 | 174 | pos_aux_right = pos_aux + [0,1] 175 | if state_matrix[pos_aux_right[0],pos_aux_right[1]] == 0: 176 | right = 1 177 | else: 178 | right = 0 179 | 180 | return acc, brk, left, right 181 | #============================================================================== 182 | # get state and front vehicle 183 | def get_state_EV(EV_set,EV_name_set,Car): # suppose car is not in EV_set and EV_name_list 184 | Car_front=None 185 | VehicleArray =np.zeros((3,3)) 186 | EV_set_aux={x for x in EV_name_set if abs(EV_set["EV{}".format(x)].lane-Car.lane)<=1 or abs(EV_set["EV{}".format(x)].lane_tg-Car.lane)<=1 or abs(EV_set["EV{}".format(x)].lane-Car.lane_tg)<=1 or abs(EV_set["EV{}".format(x)].lane_tg-Car.lane_tg)<=1} 187 | 188 | VehicleArray[1][1]=2 189 | 190 | VehicleArray[0][0]=Car.lane==0 or len({x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length-1.5*Car.grid_len*Resize)} ) is not 0 191 | VehicleArray[1][0]=Car.lane==0 or len({x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length-0.5*Car.grid_len*Resize)} ) is not 0 192 | VehicleArray[2][0]=Car.lane==0 or len({x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 193 | 194 | VehicleArray[0][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]-Car.grid_len*Resize,Car.grid_len*Resize-Car.car_length)} ) is not 0 195 | VehicleArray[2][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 196 | 197 | VehicleArray[0][2]=Car.lane==NrLanes-1 or len({x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length-1.5*Car.grid_len*Resize)} ) is not 0 198 | VehicleArray[1][2]=Car.lane==NrLanes-1 or len({x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length-0.5*Car.grid_len*Resize)} ) is not 0 199 | VehicleArray[2][2]=Car.lane==NrLanes-1 or len({x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 200 | 201 | for x in EV_set_aux: 202 | EV_set_aux2={x for x in EV_set_aux if (abs(EV_set["EV{}".format(x)].pos[0]-Car.pos[0])<=width/2.0+EV_set["EV{}".format(x)].car_width/2.0 or EV_set["EV{}".format(x)].lane_tg==Car.lane) and EV_set["EV{}".format(x)].pos[1]0: 204 | EV_pos_set={EV_set["EV{}".format(x)].pos[1] for x in EV_set_aux2} 205 | Car_front_name = list(x for x in EV_set_aux2 if EV_set["EV{}".format(x)].pos[1]==max(EV_pos_set))[0] 206 | Car_front=EV_set["EV{}".format(Car_front_name)] 207 | 208 | return VehicleArray,Car_front 209 | #============================================================================== 210 | # get state and front vehicle 211 | def get_state_HV(EV_set,EV_name_set,Car): # suppose car is not in EV_set and EV_name_list 212 | Car_front=None 213 | VehicleArray =np.zeros((3,3)) 214 | EV_set_aux={x for x in EV_name_set if abs(EV_set["EV{}".format(x)].lane-Car.lane)<=1 or abs(EV_set["EV{}".format(x)].lane_tg-Car.lane)<=1 or abs(EV_set["EV{}".format(x)].lane-Car.lane_tg)<=1 or abs(EV_set["EV{}".format(x)].lane_tg-Car.lane_tg)<=1} 215 | 216 | if Car.lane==0: 217 | VehicleArray[1][0]=2 218 | 219 | VehicleArray[0][0]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]-Car.grid_len*Resize,Car.grid_len*Resize-Car.car_length)} ) is not 0 220 | VehicleArray[2][0]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 221 | 222 | VehicleArray[0][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length-1.5*Car.grid_len*Resize)} ) is not 0 223 | VehicleArray[1][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length-0.5*Car.grid_len*Resize)} ) is not 0 224 | VehicleArray[2][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 225 | 226 | elif Car.lane==NrLanes-1: 227 | VehicleArray[1][2]=2 228 | 229 | VehicleArray[0][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length-1.5*Car.grid_len*Resize)} ) is not 0 230 | VehicleArray[1][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length-0.5*Car.grid_len*Resize)} ) is not 0 231 | VehicleArray[2][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 232 | 233 | VehicleArray[0][2]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]-Car.grid_len*Resize,Car.grid_len*Resize-Car.car_length)} ) is not 0 234 | VehicleArray[2][2]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 235 | 236 | else: 237 | VehicleArray[1][1]=2 238 | 239 | VehicleArray[0][0]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length-1.5*Car.grid_len*Resize)} ) is not 0 240 | VehicleArray[1][0]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length-0.5*Car.grid_len*Resize)} ) is not 0 241 | VehicleArray[2][0]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]-width,Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 242 | 243 | VehicleArray[0][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]-Car.grid_len*Resize,Car.grid_len*Resize-Car.car_length)} ) is not 0 244 | VehicleArray[2][1]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0],Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 245 | 246 | VehicleArray[0][2]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length-1.5*Car.grid_len*Resize)} ) is not 0 247 | VehicleArray[1][2]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length-0.5*Car.grid_len*Resize)} ) is not 0 248 | VehicleArray[2][2]=len( {x for x in EV_set_aux if overlap_area(EV_set["EV{}".format(x)],Car,Car.pos[0]+width,Car.pos[1]+0.5*Car.car_length+0.5*Car.grid_len*Resize)} ) is not 0 249 | 250 | for x in EV_set_aux: 251 | EV_set_aux2={x for x in EV_set_aux if (abs(EV_set["EV{}".format(x)].pos[0]-Car.pos[0])<=width/2.0+EV_set["EV{}".format(x)].car_width/2.0 or EV_set["EV{}".format(x)].lane_tg==Car.lane) and EV_set["EV{}".format(x)].pos[1]0: 253 | EV_pos_set={EV_set["EV{}".format(x)].pos[1] for x in EV_set_aux2} 254 | Car_front_name = list(x for x in EV_set_aux2 if EV_set["EV{}".format(x)].pos[1]==max(EV_pos_set))[0] 255 | Car_front=EV_set["EV{}".format(Car_front_name)] 256 | 257 | state_HV=matrix2state(VehicleArray) 258 | 259 | return state_HV,Car_front 260 | 261 | #============================================================================== 262 | # get center and length of EV, return overlap or not 263 | def overlap_area(EV,Car,grid_x,grid_y,delta_y=0.0): 264 | x_pos_array=[Center_x-width*2.0,Center_x-width,Center_x,Center_x+width,Center_x+width*2.0] 265 | width_aux = width/2.0+car_width/2.0+0.0 266 | if EV.Action=='left' or EV.Action=='right': 267 | x_center = (EV.pos[0]+x_pos_array[EV.lane_tg])/2.0 268 | width_aux += abs(EV.pos[0]-x_pos_array[EV.lane_tg])/2.0 269 | else: 270 | x_center = EV.pos[0] 271 | y_center = EV.pos[1]+EV.car_length/2.0-0.5*EV.grid_len*Resize-0.5*(EV.vel-Car.vel)*Car.Dt*Resize 272 | occu_len = EV.grid_len*Resize+abs(EV.vel-Car.vel)*Car.Dt*Resize 273 | 274 | return abs(x_center-grid_x)=display_height-EV_set["EV{}".format(x)].grid_len*Resize} 283 | 284 | if len(EV_set_aux1) is not 0: 285 | for i in range(NrLanes): 286 | FrontArray[i]=( len({x for x in EV_set_aux1 if overlap_area(EV_set["EV{}".format(x)],Car,Center_x+(i-2)*width,-0.5*Car.grid_len*Resize)}) is not 0 ) 287 | if len(EV_name_set)==0: 288 | last_car=1 289 | else: 290 | last_car=max(EV_name_set) 291 | indices_front = np.asarray(np.where(FrontArray == 0 )) 292 | if len(indices_front[0])>0: 293 | Nr_new_vehicle_front = rd.sample(range(len(indices_front[0])-0), 1) # pick number of cars to fill 294 | else: 295 | Nr_new_vehicle_front=[0] 296 | 297 | if Nr_new_vehicle_front[0]>0: 298 | new_vehicle_pos_front = rd.sample(range(len(indices_front[0])), Nr_new_vehicle_front[0]) 299 | k = 0 300 | for i in new_vehicle_pos_front: 301 | k = k+1 302 | EV_name_set.add(last_car+k) 303 | EV_color_index = np.random.randint(0, high=len(ColorSet)) 304 | Pos = [Center_x+(indices_front[0][i]-2)*width,-LengthSet[EV_color_index]/2.0] 305 | EV_lane=indices_front[0][i] 306 | Vel=13.0 307 | Dt=0.7 308 | EV_set["EV{}".format(last_car+k)]=EV(EV_lane, Pos, Vel, Dt, EV_color_index, Curvature_Class) 309 | Car_dict[EV_set["EV{}".format(last_car+k)].Color](Pos[0],Pos[1],EV_set["EV{}".format(last_car+k)].car_width,EV_set["EV{}".format(last_car+k)].car_length) 310 | 311 | #===================================== 312 | if len(EV_set_aux2) is not 0: 313 | for i in range(NrLanes): 314 | RearArray[i]=len( {x for x in EV_set_aux2 if overlap_area(EV_set["EV{}".format(x)],Car,Center_x+(i-2)*width,display_height+0.5*Car.grid_len*Resize)} ) is not 0 315 | 316 | if len(EV_name_set)==0: 317 | last_car=1 318 | else: 319 | last_car=max(EV_name_set) 320 | 321 | indices_rear = np.asarray(np.where(RearArray == 0 )) 322 | if len(indices_rear[0])>0: 323 | Nr_new_vehicle_rear = rd.sample(range(len(indices_rear[0])-0), 1) # pick number of cars to fill 324 | else: 325 | Nr_new_vehicle_rear=[0] 326 | 327 | if Nr_new_vehicle_rear[0]>0: 328 | new_vehicle_pos_rear = rd.sample(range(len(indices_rear[0])), Nr_new_vehicle_rear[0]) 329 | k = 0 330 | for i in new_vehicle_pos_rear: 331 | k = k+1 332 | EV_name_set.add(last_car+k) 333 | EV_color_index = np.random.randint(0, high=6) 334 | Pos = [Center_x+(indices_rear[0][i]-2)*width,display_height+Car.grid_len*Resize-LengthSet[EV_color_index]/2.0] 335 | EV_lane=indices_rear[0][i] 336 | Vel=13.0 337 | Dt=0.7 338 | EV_set["EV{}".format(last_car+k)]=EV(EV_lane, Pos, Vel, Dt, EV_color_index, Curvature_Class) 339 | Car_dict[EV_set["EV{}".format(last_car+k)].Color](Pos[0],Pos[1],EV_set["EV{}".format(last_car+k)].car_width,EV_set["EV{}".format(last_car+k)].car_length) 340 | 341 | return EV_set,EV_name_set 342 | 343 | #============================================================================== 344 | # define position mapping for curvature plotting 345 | def mapping2curve(plot_straight,plot_straigt2circuit,plot_circuit,plot_circuit2straight,x_aux,y_aux,circuit_x,circuit_y,Curvature_Class,road_y_transit,road_angle_transit,R1,R2,R3,R4,R5,display_width,display_height): 346 | if plot_straight == True: 347 | x_aux2 = x_aux+0 348 | y_aux2 = y_aux+0 349 | angle_aux2 = 0 350 | elif plot_straight == False and plot_straigt2circuit == True: 351 | if y_aux>=circuit_y: 352 | x_aux2 = x_aux+0 353 | y_aux2 = y_aux+0 354 | angle_aux2 = 0 355 | elif y_auxcircuit_y: # below circuit bottom 383 | delta_L = y_aux-circuit_y 384 | delta_angle = delta_L/R3 385 | if road_angle_transit>=0: 386 | delta_x = -R3*(1-mh.cos(delta_angle)) 387 | angle_aux2 = -delta_angle/np.pi*180 388 | elif road_angle_transit<0: 389 | delta_x = R3*(1-mh.cos(delta_angle)) 390 | angle_aux2 = delta_angle/np.pi*180 391 | delta_y = R3*mh.sin(delta_angle)-delta_L 392 | x_aux2 = x_aux+delta_x 393 | y_aux2 = y_aux+delta_y 394 | elif display_height-y_aux=0: 398 | delta_x = -R3*(1-mh.cos(delta_angle)) 399 | angle_aux2 = delta_angle/np.pi*180 400 | elif road_angle_transit<0: 401 | delta_x = R3*(1-mh.cos(delta_angle)) 402 | angle_aux2 = -delta_angle/np.pi*180 403 | delta_y = delta_L-R3*mh.sin(delta_angle) 404 | x_aux2 = x_aux+delta_x 405 | y_aux2 = y_aux+delta_y 406 | elif display_height-y_aux>=np.abs(road_angle_transit/180*np.pi)*R3: # in straight 407 | delta_L = (display_height-y_aux)-np.abs(road_angle_transit/180*np.pi)*R3 408 | if road_angle_transit>=0: 409 | delta_x = -R3*(1-mh.cos(road_angle_transit/180*np.pi))-delta_L*mh.sin(road_angle_transit/180*np.pi) 410 | elif road_angle_transit<0: 411 | delta_x = R3*(1-mh.cos(road_angle_transit/180*np.pi))-delta_L*mh.sin(road_angle_transit/180*np.pi) 412 | delta_y = -delta_x*mh.tan(road_angle_transit/180*np.pi) 413 | x_aux2 = x_aux+delta_x 414 | y_aux2 = y_aux+delta_y 415 | angle_aux2 = road_angle_transit 416 | 417 | return x_aux2,y_aux2,angle_aux2 418 | 419 | #============================================================================== 420 | def bound(x,x1,x2): 421 | return min( max(x,x1), x2 ) 422 | 423 | #============================================================================== 424 | def path_planning(pos0,k_max,Lane_width,direction): #direction>0 -> left 425 | K=(( (1+144/Lane_width**2/k_max**2)**0.5-1 )/2)**0.5 426 | Length=K*Lane_width 427 | D0 = (1+K**2)**0.5/4/K*Lane_width 428 | L0 = 2*K*D0 429 | 430 | XI0_offset=pos0[0]/Resize 431 | YI0_offset=pos0[1]/Resize 432 | 433 | XI0=0 434 | YI0=0 435 | XIf=XI0-direction*D0 436 | YIf=YI0+L0 437 | 438 | P0 = np.zeros(2) 439 | P0[0] = XI0 440 | P0[1] = YI0 441 | 442 | P4 = np.zeros(2) 443 | P4[0] = XIf 444 | P4[1] = YIf 445 | 446 | P2 = (P0+P4)/2 447 | 448 | Arr = L0*0.25 449 | 450 | P1 = P0+[0, Arr] 451 | P3 = P4-[0, Arr] 452 | 453 | dtau = 0.02 454 | Number = int(round(1/dtau))+1 455 | tau = np.linspace(0, 1, num=Number) 456 | 457 | Aux1 = (1-tau)**4*P0[0]+4*(1-tau)**3*tau*P1[0]+6*(1-tau)**2*tau**2*P2[0]+4*(1-tau)*tau**3*P3[0]+tau**4*P4[0] 458 | Aux2 = (1-tau)**4*P0[1]+4*(1-tau)**3*tau*P1[1]+6*(1-tau)**2*tau**2*P2[1]+4*(1-tau)*tau**3*P3[1]+tau**4*P4[1] 459 | 460 | Curve_AB = np.zeros((Number,2)) 461 | Curve_AB[:,0] = Aux1 462 | Curve_AB[:,1] = Aux2 463 | 464 | D_x = 4*(1-tau)**3*(P1[0]-P0[0])+12*(1-tau)**2*tau*(P2[0]-P1[0])+12*tau**2*(1-tau)*(P3[0]-P2[0])+4*tau**3*(P4[0]-P3[0]) 465 | D_y = 4*(1-tau)**3*(P1[1]-P0[1])+12*(1-tau)**2*tau*(P2[1]-P1[1])+12*tau**2*(1-tau)*(P3[1]-P2[1])+4*tau**3*(P4[1]-P3[1]) 466 | 467 | DD_x = 12*(1-tau)**2*(P2[0]-2*P1[0]+P0[0])+24*tau*(1-tau)*(P3[0]-2*P2[0]+P1[0])+12*tau**2*(P4[0]-2*P3[0]+P2[0]) 468 | DD_y = 12*(1-tau)**2*(P2[1]-2*P1[1]+P0[1])+24*tau*(1-tau)*(P3[1]-2*P2[1]+P1[1])+12*tau**2*(P4[1]-2*P3[1]+P2[1]) 469 | 470 | ka = (D_x*DD_y-D_y*DD_x)/( (D_x**2+D_y**2)**1.5 ) 471 | 472 | tau_ex = np.linspace(0, 2, num=int(round(2/dtau))+1)/2 473 | mid=int((len(tau)+1)/2-1) 474 | Aux_1 = np.append(Aux1[mid:0:-1],Aux1) 475 | Aux_1 = np.append(Aux_1,Aux1[len(tau)-2:mid-1:-1]) 476 | 477 | Aux_2 = np.append(-Aux2[mid:0:-1],Aux2) 478 | Aux_2 = np.append(Aux_2,2*L0-Aux2[len(tau)-2:mid-1:-1]) 479 | Curve_AB_Ex = np.zeros((int(round(2/dtau))+1,2)) 480 | Curve_AB_Ex[:,0] = Aux_1 481 | Curve_AB_Ex[:,1] = Aux_2 482 | 483 | ka_Ex = np.append(ka[mid:0:-1],ka) 484 | ka_Ex = np.append(ka_Ex,ka[len(tau)-2:mid-1:-1]) 485 | 486 | # coordinate transform 487 | aux_1 = Aux_1+direction*D0/2 488 | aux_2 = Aux_2+L0/2; 489 | 490 | # theta = np.pi/2-atan(L0/D0/2); #Cooridnate CW 491 | theta = atan(1/K)*direction 492 | 493 | Aux_1 = aux_1*mh.cos(theta)-aux_2*mh.sin(theta); 494 | Aux_2 = aux_1*mh.sin(theta)+aux_2*mh.cos(theta); 495 | Curve_AB_Ex_Rotate = np.zeros((int(round(2/dtau))+1,2)) 496 | Curve_AB_Ex_Rotate[:,0] = Aux_1+XI0_offset 497 | Curve_AB_Ex_Rotate[:,1] = Aux_2+YI0_offset 498 | 499 | # transformed D_x, D_y 500 | Aux_1 = np.append(-D_x[mid:0:-1],D_x) 501 | aux_1 = np.append(Aux_1,-D_x[len(tau)-2:mid-1:-1]) 502 | 503 | Aux_2 = np.append(D_y[mid:0:-1],D_y) 504 | aux_2 = np.append(Aux_2,D_y[len(tau)-2:mid-1:-1]) 505 | 506 | Aux_1 = aux_1*mh.cos(theta)-aux_2*mh.sin(theta) 507 | Aux_2 = aux_1*mh.sin(theta)+aux_2*mh.cos(theta) 508 | psi_t = atan2(Aux_2,Aux_1) 509 | 510 | # plt.plot(Curve_AB_Ex_Rotate[:,0], Curve_AB_Ex_Rotate[:,1], 'ro') 511 | # plt.show() 512 | 513 | return tau_ex, Curve_AB_Ex_Rotate, ka_Ex, psi_t, Length 514 | 515 | 516 | #=============================================== 517 | class EV(object): 518 | def __init__(self, EV_lane, Pos, Vel, Dt, color_index, Curvature_Class, m=850, h=0.5, lf=1.5, lr=0.9, Iz=1401): 519 | self.Pr_stay = Pr_actions[-1] 520 | self.Pr_acc = Pr_actions[-2] 521 | self.Pr_left = Pr_actions[-3] 522 | self.Pr_right = Pr_actions[-4] 523 | self.Pr_brake = Pr_actions[-5] 524 | 525 | self.lane = EV_lane 526 | self.lane_tg = EV_lane 527 | self.pos = Pos #[bit] 528 | self.vel = Vel*1.6/3.6 #[m/s] 529 | self.vel_tg = Vel*1.6/3.6 #[m/s] 530 | self.acc = 0.0 531 | self.Dt = Dt 532 | self.Action = 'stay' 533 | self.in_action = False 534 | self.Curvature_Class = Curvature_Class 535 | self.color_index = color_index 536 | self.Color = ColorSet[self.color_index] 537 | self.car_width=WidthSet[self.color_index] 538 | self.car_length=LengthSet[self.color_index] 539 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 540 | self.V_max = 40*1.6/3.6 #Resize=130/4.5 541 | self.V_min = 25*1.6/3.6 542 | self.acc_up = 5; #m/s^2 543 | self.acc_low = -12; #m/s^2 544 | 545 | self.VehicleArray=np.zeros((3,3)) 546 | self.move_count = 0 547 | self.move_count_pure_speeding = 0 548 | 549 | # self.nu = -0.2 550 | # self.nu1 = -0.2 551 | # self.nu2 = -0.1 552 | self.nu = -1 553 | self.nu1 = -2 554 | self.nu2 = -1 555 | 556 | self.nu = -5 557 | self.nu1 = -10 558 | self.nu2 = -5 559 | 560 | self.nu = -5 561 | self.nu1 = -20 562 | self.nu2 = -10 563 | 564 | ## vehicle dynamics parameters 565 | self.m = m 566 | self.h = h 567 | self.lf = lf 568 | self.lr = lr 569 | self.Iz = Iz 570 | self.Iwr = 1.2 571 | self.r_F = 0.311 572 | self.r_R = 0.311 573 | self.L_axis = 2.4 574 | self.ls = 0.01 575 | 576 | self.Cf = 42000 577 | self.Cr = 81000 578 | self.nt = 0.225 579 | 580 | self.B = 20 581 | self.C = 1.6 # C<2 582 | self.D = 1.3 #0.715 583 | self.E = 0.0 #-0.064 584 | self.Sh = 0.0 #-0.011 585 | self.Sv = 0.0 #0.01 586 | 587 | self.Vx = self.vel+0.0 588 | self.Vy = 0.0 589 | self.r = 0.0 590 | self.psi = np.pi/2 591 | self.wR = self.vel/self.r_R-1e-3 592 | 593 | ##### define vehicle dynamics 594 | def vehicle_dynamics(self,HV,delta,Tr_aux,dt): 595 | Vf=[self.Vx, self.Vy, 0]+ np.cross([0, 0, self.r], [self.lf, 0, self.r_F-self.h]); 596 | Vr=[self.Vx, self.Vy, 0]+ np.cross([0, 0, self.r], [-self.lr, 0, self.r_R-self.h]); 597 | # wheel velocity in wheel-fixed frame 598 | Tran=np.array([[mh.cos(delta), mh.sin(delta), 0], [-mh.sin(delta), mh.cos(delta), 0], [0, 0, 1]]) 599 | Vf=Tran.dot(Vf) 600 | 601 | VFx=Vf[0]; 602 | VFy=Vf[1]; 603 | VRx=Vr[0]; 604 | VRy=Vr[1]; 605 | 606 | # bound the the control torque 607 | beta_R=atan(VRy/VRx) 608 | s_R = np.tan(np.pi/2/self.C)/self.B 609 | if abs(mh.tan(beta_R))>s_R: 610 | beta_R=mh.atan(s_R/2.0) 611 | mu_R = self.D 612 | 613 | fRx_up = self.m*g*self.lf/( self.L_axis*s_R*(1/mh.cos(beta_R))**2/mu_R/( ( (1/mh.cos(beta_R))**2*s_R**2-(np.tan(beta_R))**2 )**0.5+(np.tan(beta_R))**2 )-self.h ) 614 | fRx_low = -self.m*g*self.lf/( self.L_axis*s_R*(1/mh.cos(beta_R))**2/mu_R/( ( (1/mh.cos(beta_R))**2*s_R**2-(np.tan(beta_R))**2 )**0.5-(np.tan(beta_R))**2 )+self.h ) 615 | Tr_up = fRx_up*self.r_R*(self.m+self.Iwr/self.r_R**2)/self.m 616 | Tr_low = fRx_low*self.r_R*(self.m+self.Iwr/self.r_R**2)/self.m 617 | 618 | Tr = bound( Tr_aux,0.95*Tr_low,0.95*Tr_up ) 619 | 620 | if abs(self.Vx)<0.05: #abs(self.wR*self.r_R)<0.05: # case of low velocity 621 | 622 | dot_Vy=0; 623 | dot_r=0; 624 | dot_XI=0; 625 | dot_wR=Tr/self.Iwr; 626 | dot_Vx=0.8*dot_wR*self.r_R; 627 | else: 628 | sFx=0 629 | sFy=VFy/VFx 630 | sF= ( sFx**2+sFy**2 )**0.5 631 | 632 | sRx=( VRx-self.wR*self.r_R )/abs( self.wR*self.r_R ) 633 | sRy=VRy/abs( self.wR*self.r_R ) 634 | sR= ( sRx**2+sRy**2 )**0.5; 635 | 636 | mu_sF=self.D*mh.sin( self.C*atan( self.B*( (1-self.E)*(sF+self.Sh)+self.E/self.B*atan(self.B*(sF+self.Sh)) ) ) ) +self.Sv; 637 | mu_sR=self.D*mh.sin( self.C*atan( self.B*( (1-self.E)*(sR+self.Sh)+self.E/self.B*atan(self.B*(sR+self.Sh)) ) ) ) +self.Sv; 638 | 639 | if sF==0: # no slip at front wheel 640 | fFx=0; 641 | fFy=0; 642 | muRx=-sRx*mu_sR/sR; 643 | muRy=-sRy*mu_sR/sR; 644 | # calculate rear tire forces 645 | fFz=( self.lr-self.h*muRx )*self.m*g/( self.L_axis-self.h*muRx ); 646 | fRz=self.m*g-fFz; 647 | fRx=muRx*fRz; 648 | fRy=muRy*fRz; 649 | else: 650 | muFx=-sFx*mu_sF/sF; 651 | muFy=-sFy*mu_sF/sF; 652 | muRx=-sRx*mu_sR/sR; 653 | muRy=-sRy*mu_sR/sR; 654 | 655 | # calculate tire forces 656 | fFz=( self.lr-self.h*muRx )*self.m*g/( self.L_axis+self.h*( muFx*mh.cos( delta )-muFy*mh.sin( delta )- muRx) ); 657 | fRz=self.m*g-fFz; 658 | 659 | fFx=muFx*fFz; 660 | fFy=muFy*fFz; 661 | fRx=muRx*fRz; 662 | fRy=muRy*fRz; 663 | 664 | dot_Vx=( fFx*mh.cos( delta )-fFy*mh.sin( delta )+fRx )/self.m+self.Vy*self.r; 665 | dot_Vy=( fFx*mh.sin( delta )+fFy*mh.cos( delta )+fRy )/self.m-self.Vx*self.r; 666 | dot_r=( ( fFy*mh.cos( delta )+fFx*mh.sin( delta ) )*self.lf-fRy*self.lr )/self.Iz; 667 | 668 | T_rotation=np.array([ [mh.cos(self.psi), mh.sin(self.psi)], [-mh.sin(self.psi), mh.cos(self.psi)] ]) 669 | dot_pos=T_rotation.transpose().dot([ self.Vx, self.Vy ]) 670 | 671 | dot_XI=dot_pos[0]; 672 | # dot_YI=dot_pos[1]; # do not update longitudinal pos for HV 673 | 674 | dot_wR=(Tr-fRx*self.r_R)/self.Iwr; 675 | 676 | self.Vx += dot_Vx*dt 677 | self.Vy += dot_Vy*dt 678 | self.psi += self.r*dt 679 | self.wR += dot_wR*dt 680 | self.wR = max(1e-1,self.wR) 681 | 682 | self.r += dot_r*dt 683 | self.pos[0] += dot_XI*dt*Resize 684 | self.pos[1] -= (self.Vx-HV.Vx)*dt*Resize 685 | 686 | self.Vx=bound(self.Vx,self.V_min,self.V_max) 687 | self.vel = self.Vx+0.0 688 | 689 | ##### define controller 690 | def ORT(self): 691 | ## Vx_aux = 15.0 692 | # Vx_aux = max(self.Vx, 5) 693 | # A=np.matrix([[-(self.Cf+self.Cr)/Vx_aux/self.m,-1+(self.lr*self.Cr-self.lf*self.Cf)/self.m/Vx_aux**2,0,0],[(self.lr*self.Cr-self.lf*self.Cf)/self.Iz,-(self.lr**2*self.Cr+self.lf**2*self.Cf)/self.Iz/Vx_aux,0,0],[-Vx_aux,-self.ls,0,Vx_aux],[0,-1,0,0]]) 694 | # B=np.matrix([self.Cf/self.m/Vx_aux,self.lf*self.Cf/self.Iz,0,0]).T 695 | # E=np.matrix([0,0,Vx_aux*self.ls,Vx_aux]).T 696 | # C=np.matrix([0,0,1,0]) 697 | # 698 | # rows = A.shape[0] 699 | # P = cp.Variable((rows, rows), PSD=True) 700 | # S = cp.Variable((1,rows)) 701 | # PI = cp.Variable((rows,1)) 702 | # 703 | # Gamma = cp.Variable() 704 | # epsi = cp.Variable() 705 | # I = np.identity(rows) 706 | # 707 | # cons1 = epsi>=0 708 | # cons2 = -(A*P+B*S+P.T*A.T+S.T*B.T)>>1e0 709 | # cons3 = cp.bmat([[-I, A*PI+B*Gamma+E],[PI.T*A.T+Gamma.T*B.T+E.T, -epsi*np.eye(1)]])<<1e-10 710 | # cons4 = cp.bmat([[-epsi*np.eye(1), C*PI],[PI.T*C.T, -np.identity(1)]])<<1e-10 711 | # cons5 = P>>1e1 712 | ## cons6 = epsi-1e-5<=0 713 | # 714 | # optprob = cp.Problem(cp.Minimize(epsi), constraints=[cons1, cons2, cons3, cons4, cons5]) 715 | # result = optprob.solve() 716 | # 717 | # aux1 = S.value.dot(np.linalg.pinv(P.value)) 718 | # aux2 = Gamma.value-aux1.dot(PI.value) 719 | # self.G = aux1[0] 720 | # self.H = aux2[0][0] 721 | 722 | # self.G = np.array([-0.171908272474611,-0.0194559249021821,0.0686712004081447,0.972969648170903]) 723 | # self.H = 4.58197066088387 724 | 725 | self.G = np.array([-3.464761346737013525e+00,-7.421571597432523593e-01,1.132624286600968144e+00,4.632126864955187884e+00]) 726 | self.H = 14.436298476877123 727 | 728 | # self.G = np.array([-0.171908282309310,-0.0194559256488496,0.0686712014555834,0.972969665762630]) 729 | # self.H = 4.46361884075154 730 | 731 | ##### implement action 732 | def maintain_fcn(self,car_front,HV,steps=np.int(3*maintain_Time/dt)): 733 | if self.in_action: 734 | self.move_count+=1 735 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 736 | 737 | # yaw_err = self.psi-np.pi/2 738 | delta_y=self.pos[0]/Resize+self.ls*mh.cos(self.psi)-(Center_x+(self.lane-2)*width)/Resize; 739 | delta_psi = np.pi/2-self.psi; 740 | # self.ORT() 741 | delta = self.G.dot(np.array([ atan(self.Vy/self.Vx), self.r, delta_y, delta_psi ])) # check math 742 | 743 | if car_front is not None: 744 | dist=-(car_front.pos[1]-self.pos[1])/Resize 745 | 746 | if car_front==None: 747 | v_err=self.vel-self.vel_tg 748 | # self.acc = bound( self.nu*v_err,self.acc_low,self.acc_up ) 749 | self.acc = self.nu*v_err 750 | elif self.vel>=car_front.vel and dist<2*self.grid_len: 751 | self.vel_tg=car_front.vel+0.0 752 | s_err=-dist+self.grid_len 753 | v_err=self.vel-self.vel_tg 754 | # self.acc = bound( self.nu1*v_err+self.nu2*s_err,self.acc_low,self.acc_up ) 755 | self.acc = self.nu1*v_err+self.nu2*s_err 756 | else: #if self.vel=steps) and delta_psi**2<25e-4 and self.r**2<1e-2: 767 | self.move_count=0 768 | self.in_action=False 769 | 770 | def acc_fcn(self,car_front,HV,acc,steps=np.int(3*maintain_Time/dt)): 771 | if self.in_action: 772 | # yaw_err = self.psi-np.pi/2 773 | delta_y=self.pos[0]/Resize+self.ls*mh.cos(self.psi)-(Center_x+(self.lane-2)*width)/Resize; 774 | delta_psi = np.pi/2-self.psi; 775 | # self.ORT() 776 | delta = self.G.dot(np.array([ atan(self.Vy/self.Vx), self.r, delta_y, delta_psi ])) # check math 777 | 778 | if acc or car_front==None: 779 | self.move_count_pure_speeding+=1 780 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 781 | self.vel_tg=self.V_max 782 | v_err=self.vel-self.vel_tg 783 | self.acc = self.nu*v_err 784 | # self.acc=self.acc_up 785 | else: # provide the car making acc==0 786 | self.move_count+=1 787 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 788 | dist=-(car_front.pos[1]-self.pos[1])/Resize 789 | self.vel_tg=car_front.vel 790 | s_err=-dist+self.grid_len 791 | v_err=self.vel-self.vel_tg 792 | self.acc = self.nu1*v_err+self.nu2*s_err 793 | # self.vel+=self.acc*dt 794 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 795 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 796 | self.vehicle_dynamics(HV,delta,Tr_aux,dt) 797 | # self.vel=bound(self.vel,self.V_min,self.V_max) 798 | # self.pos[1] -= (self.vel-HV.vel)*dt*Resize # not update y_pos 799 | if (v_err**2<0.25 or self.move_count_pure_speeding>=steps or self.move_count>=steps) and delta_psi**2<25e-4 and self.r**2<1e-2: 800 | self.move_count_pure_speeding=0 801 | self.move_count=0 802 | self.in_action=False 803 | 804 | def brk_fcn(self,car_front,HV,steps=np.int(maintain_Time/dt)): 805 | if self.in_action: 806 | self.move_count+=1 807 | # yaw_err = self.psi-np.pi/2 808 | delta_y=self.pos[0]/Resize+self.ls*mh.cos(self.psi)-(Center_x+(self.lane-2)*width)/Resize; 809 | delta_psi = np.pi/2-self.psi; 810 | # self.ORT() 811 | delta = self.G.dot(np.array([ atan(self.Vy/self.Vx), self.r, delta_y, delta_psi ])) # check math 812 | 813 | if car_front is not None and self.vel>car_front.vel: 814 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 815 | dist=-(car_front.pos[1]-self.pos[1])/Resize 816 | vel_aux=max(self.V_min,min(car_front.vel,self.vel_tg*0.8)) 817 | s_err=-dist+self.grid_len 818 | v_err=self.vel-vel_aux 819 | self.acc = self.nu1*v_err+self.nu2*s_err 820 | # self.acc = bound( self.nu1*v_err+self.nu2*s_err,self.acc_low,self.acc_up ) 821 | else: 822 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 823 | v_err=self.vel-self.vel_tg*0.8 824 | self.acc = self.nu*v_err 825 | # self.acc = bound( self.nu*v_err,self.acc_low,self.acc_up ) 826 | # self.vel+=self.acc*dt 827 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 828 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 829 | self.vehicle_dynamics(HV,delta,Tr_aux,dt) 830 | # self.vel=bound(self.vel,self.V_min,self.V_max) 831 | # self.pos[1] -= (self.vel-HV.vel)*dt*Resize # not update y_pos 832 | if (v_err**2<0.25 or self.move_count>=steps) and delta_psi**2<25e-4 and self.r**2<1e-2: 833 | self.move_count=0 834 | self.in_action=False 835 | 836 | def left_fcn(self,car_front,HV): 837 | if self.in_action: 838 | lat_err = self.pos[0]/Resize-self.Curve_AB_Ex_Rotate[-1,0] 839 | yaw_err = self.psi-np.pi/2 840 | pos_pred = np.zeros(2) 841 | pos_pred[0]=self.pos[0]/Resize+self.ls*mh.cos(self.psi) 842 | pos_pred[1]=(2*self.Curve_AB_Ex_Rotate[0,1]-self.pos[1]/Resize)+self.ls*mh.sin(self.psi) 843 | travel_dis = pos_pred[1]-self.Curve_AB_Ex_Rotate[0,1] 844 | if travel_discar_front.vel: 861 | dist=-(car_front.pos[1]-self.pos[1])/Resize 862 | self.vel_tg=car_front.vel+0 863 | s_err=-dist+self.grid_len 864 | v_err=self.vel-self.vel_tg 865 | self.acc = self.nu1*v_err+self.nu2*s_err 866 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 867 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 868 | 869 | self.vehicle_dynamics(HV,delta,Tr_aux,dt) 870 | 871 | self.Curve_AB_Ex_Rotate[:,1]=self.Curve_AB_Ex_Rotate[:,1]+HV.Vx*dt 872 | if lat_err**2<=1e-2 and yaw_err**2<25e-4 and self.r**2<1e-2:#travel_dis>=extra_dis+self.Length: #lat_err**2<=1e-2: 873 | # self.move_count=0 874 | self.in_action=False 875 | self.lane = self.lane_tg+0 876 | 877 | 878 | def right_fcn(self,car_front,HV): 879 | if self.in_action: 880 | lat_err = self.pos[0]/Resize-self.Curve_AB_Ex_Rotate[-1,0] 881 | yaw_err = self.psi-np.pi/2 882 | pos_pred = np.zeros(2) 883 | pos_pred[0]=self.pos[0]/Resize+self.ls*mh.cos(self.psi) 884 | pos_pred[1]=(2*self.Curve_AB_Ex_Rotate[0,1]-self.pos[1]/Resize)+self.ls*mh.sin(self.psi) 885 | travel_dis = pos_pred[1]-self.Curve_AB_Ex_Rotate[0,1] 886 | if travel_discar_front.vel: 903 | dist=-(car_front.pos[1]-self.pos[1])/Resize 904 | self.vel_tg=car_front.vel+0 905 | s_err=-dist+self.grid_len 906 | v_err=self.vel-self.vel_tg 907 | self.acc = self.nu1*v_err+self.nu2*s_err 908 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 909 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 910 | 911 | self.vehicle_dynamics(HV,delta,Tr_aux,dt) 912 | # self.vel=bound(self.vel,self.V_min,self.V_max) 913 | 914 | # self.Curve_AB_Ex_Rotate[:,1]+=HV.Vx*dt 915 | self.Curve_AB_Ex_Rotate[:,1]=self.Curve_AB_Ex_Rotate[:,1]+HV.Vx*dt 916 | if lat_err**2<=1e-2 and yaw_err**2<25e-4 and self.r**2<1e-2:#travel_dis>=extra_dis+self.Length: #lat_err**2<=1e-2: 917 | # self.move_count=0 918 | self.in_action=False 919 | self.lane = self.lane_tg+0 920 | 921 | def TakeAction(self,state,car_front,HV): # use new state when take action everytime, update lane and pos 922 | 923 | self.Pr_stay = Pr_actions[-1] 924 | self.Pr_acc = Pr_actions[-2] 925 | self.Pr_left = Pr_actions[-3] 926 | self.Pr_right = Pr_actions[-4] 927 | self.Pr_brake = Pr_actions[-5] 928 | 929 | acc, brk, left, right = Available_actions_EV(state) 930 | Pr_sum = sum([brk, right, left, acc, 1]*Pr_actions) # total = 1 if everything available 931 | 932 | self.Pr_stay = self.Pr_stay/Pr_sum 933 | self.Pr_acc = self.Pr_acc/Pr_sum*acc # ->0 if acc=0 934 | self.Pr_left = self.Pr_left/Pr_sum*left 935 | self.Pr_right = self.Pr_right/Pr_sum*right 936 | self.Pr_brake = self.Pr_brake/Pr_sum*brk 937 | 938 | if not self.in_action: 939 | act = np.random.random() # generate an action number randomly 940 | eps = 1e-8 941 | aux1 = self.Pr_stay+eps 942 | aux2 = aux1+self.Pr_acc+eps 943 | aux3 = aux2+self.Pr_brake+eps 944 | aux4 = aux3+self.Pr_left+eps 945 | aux5 = aux4+self.Pr_right+eps # aux5 == 1+5*eps 946 | bins = np.array([0, aux1, aux2, aux3, aux4, aux5]) 947 | index = np.digitize(act,bins) 948 | self.Action = ActionSet[index-1] 949 | 950 | if self.Action=='left' or self.Action=='right': 951 | if self.Action=='left': 952 | direction = 1.0 953 | else: 954 | direction = -1.0 955 | k_max = min(1.0/150, 0.25*self.D*g/self.Vx**2) 956 | self.tau_ex, self.Curve_AB_Ex_Rotate, self.ka_Ex, self.psi_t, self.Length = path_planning(self.pos,k_max,width/Resize,direction) 957 | 958 | self.ORT() 959 | 960 | self.vel_tg = self.vel+0.0 961 | self.in_action = True 962 | 963 | 964 | if self.Action=='stay': 965 | self.maintain_fcn(car_front,HV) 966 | elif self.Action=='acc': 967 | self.acc_fcn(car_front,HV,acc) 968 | elif self.Action=='brk': 969 | self.brk_fcn(car_front,HV) 970 | elif self.Action=='left': 971 | self.lane_tg = self.lane-1 972 | self.left_fcn(car_front,HV) 973 | else: 974 | self.lane_tg = self.lane+1 975 | self.right_fcn(car_front,HV) 976 | 977 | #=============================================== 978 | class HV_obj(object): 979 | def __init__(self, HV_lane, Pos, Vel, Dt, Policy, color_index, Curvature_Class, m=850, h=0.5, lf=1.5, lr=0.9, Iz=1401): # for demonstraion purpose, check available actions so that no collision happens 980 | # modify Available_actions_fn with all actions for other purpose 981 | self.lane = HV_lane 982 | self.lane_tg = HV_lane 983 | self.pos = Pos #[bit] 984 | self.vel = Vel*1.6/3.6 #[m/s] 985 | self.vel_tg = Vel*1.6/3.6 #[m/s] 986 | self.acc = 0.0 987 | self.Dt = Dt 988 | self.Action = 'stay' 989 | self.in_action = False 990 | self.Curvature_Class = Curvature_Class 991 | self.color_index = color_index 992 | self.Color = ColorSet_HV[self.color_index] 993 | self.car_width=car_width 994 | self.car_length=car_length 995 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 996 | self.Policy = Policy 997 | self.V_max = 45*1.6/3.6 #Resize=130/4.5 998 | self.V_min = 18*1.6/3.6 999 | self.acc_up = 5; #m/s^2 1000 | self.acc_low = -12; #m/s^2 1001 | 1002 | self.state=9*[0] 1003 | self.move_count = 0 1004 | self.move_count_pure_speeding = 0 1005 | 1006 | # self.nu = -0.1 1007 | # self.nu1 = -0.2 1008 | # self.nu2 = -0.1 1009 | self.nu = -1 1010 | self.nu1 = -2 1011 | self.nu2 = -1 1012 | 1013 | self.nu = -5 1014 | self.nu1 = -10 1015 | self.nu2 = -5 1016 | 1017 | self.nu = -5 1018 | self.nu1 = -20 1019 | self.nu2 = -10 1020 | ## vehicle dynamics parameters 1021 | self.m = m 1022 | self.h = h 1023 | self.lf = lf 1024 | self.lr = lr 1025 | self.Iz = Iz 1026 | self.Iwr = 1.2 1027 | self.r_F = 0.311 1028 | self.r_R = 0.311 1029 | self.L_axis = 2.4 1030 | self.ls = 0.01 1031 | 1032 | self.Cf = 42000 1033 | self.Cr = 81000 1034 | self.nt = 0.225 1035 | 1036 | self.B = 20 1037 | self.C = 1.6 # C<2 1038 | self.D = 1.2 #0.715 1039 | self.E = 0.0 #-0.064 1040 | self.Sh = 0.0 #-0.011 1041 | self.Sv = 0.0 #0.01 1042 | 1043 | self.Vx = self.vel+0.0 1044 | self.Vy = 0.0 1045 | self.r = 0.0 1046 | self.psi = np.pi/2 1047 | self.wR = self.vel/self.r_R-1e-3 1048 | 1049 | def vehicle_dynamics(self,delta,Tr_aux,dt): 1050 | Vf=[self.Vx, self.Vy, 0]+ np.cross([0, 0, self.r], [self.lf, 0, self.r_F-self.h]); 1051 | Vr=[self.Vx, self.Vy, 0]+ np.cross([0, 0, self.r], [-self.lr, 0, self.r_R-self.h]); 1052 | # wheel velocity in wheel-fixed frame 1053 | Tran=np.array([[mh.cos(delta), mh.sin(delta), 0], [-mh.sin(delta), mh.cos(delta), 0], [0, 0, 1]]) 1054 | Vf=Tran.dot(Vf) 1055 | 1056 | VFx=Vf[0]; 1057 | VFy=Vf[1]; 1058 | VRx=Vr[0]; 1059 | VRy=Vr[1]; 1060 | 1061 | # bound the the control torque 1062 | beta_R=atan(VRy/VRx) 1063 | s_R = np.tan(np.pi/2/self.C)/self.B 1064 | if abs(mh.tan(beta_R))>s_R: 1065 | beta_R=mh.atan(s_R/2.0) 1066 | mu_R = self.D 1067 | 1068 | fRx_up = self.m*g*self.lf/( self.L_axis*s_R*(1/mh.cos(beta_R))**2/mu_R/( ( (1/mh.cos(beta_R))**2*s_R**2-(np.tan(beta_R))**2 )**0.5+(np.tan(beta_R))**2 )-self.h ) 1069 | fRx_low = -self.m*g*self.lf/( self.L_axis*s_R*(1/mh.cos(beta_R))**2/mu_R/( ( (1/mh.cos(beta_R))**2*s_R**2-(np.tan(beta_R))**2 )**0.5-(np.tan(beta_R))**2 )+self.h ) 1070 | Tr_up = fRx_up*self.r_R*(self.m+self.Iwr/self.r_R**2)/self.m 1071 | Tr_low = fRx_low*self.r_R*(self.m+self.Iwr/self.r_R**2)/self.m 1072 | Tr = bound( Tr_aux,0.95*Tr_low,0.95*Tr_up ) 1073 | if self.wR<0: 1074 | stop=0 1075 | 1076 | if abs(self.Vx)<0.05: #abs(self.wR*self.r_R)<0.05: # case of low velocity 1077 | 1078 | dot_Vy=0; 1079 | dot_r=0; 1080 | dot_XI=0; 1081 | # dot_YI=0; 1082 | dot_wR=Tr/self.Iwr; 1083 | dot_Vx=0.8*dot_wR*self.r_R; 1084 | else: 1085 | sFx=0 1086 | sFy=VFy/VFx 1087 | sF= ( sFx**2+sFy**2 )**0.5 1088 | sRx=( VRx-self.wR*self.r_R )/abs( self.wR*self.r_R ) 1089 | sRy=VRy/abs( self.wR*self.r_R ) 1090 | sR= ( sRx**2+sRy**2 )**0.5; 1091 | 1092 | mu_sF=self.D*mh.sin( self.C*atan( self.B*sF ) ) 1093 | mu_sR=self.D*mh.sin( self.C*atan( self.B*sR ) ) 1094 | 1095 | if sF==0: # no slip at front wheel 1096 | fFx=0; 1097 | fFy=0; 1098 | muRx=-sRx*mu_sR/sR; 1099 | muRy=-sRy*mu_sR/sR; 1100 | # calculate rear tire forces 1101 | fFz=( self.lr-self.h*muRx )*self.m*g/( self.L_axis-self.h*muRx ); 1102 | fRz=self.m*g-fFz; 1103 | fRx=muRx*fRz; 1104 | fRy=muRy*fRz; 1105 | else: 1106 | muFx=-sFx*mu_sF/sF; 1107 | muFy=-sFy*mu_sF/sF; 1108 | muRx=-sRx*mu_sR/sR; 1109 | muRy=-sRy*mu_sR/sR; 1110 | 1111 | # calculate tire forces 1112 | fFz=( self.lr-self.h*muRx )*self.m*g/( self.L_axis+self.h*( muFx*mh.cos( delta )-muFy*mh.sin( delta )- muRx) ); 1113 | fRz=self.m*g-fFz; 1114 | 1115 | fFx=muFx*fFz; 1116 | fFy=muFy*fFz; 1117 | fRx=muRx*fRz; 1118 | fRy=muRy*fRz; 1119 | 1120 | dot_Vx=( fFx*mh.cos( delta )-fFy*mh.sin( delta )+fRx )/self.m+self.Vy*self.r; 1121 | dot_Vy=( fFx*mh.sin( delta )+fFy*mh.cos( delta )+fRy )/self.m-self.Vx*self.r; 1122 | dot_r=( ( fFy*mh.cos( delta )+fFx*mh.sin( delta ) )*self.lf-fRy*self.lr )/self.Iz; 1123 | 1124 | T_rotation=np.array([ [mh.cos(self.psi), mh.sin(self.psi)], [-mh.sin(self.psi), mh.cos(self.psi)] ]) 1125 | dot_pos=T_rotation.transpose().dot([ self.Vx, self.Vy ]) 1126 | 1127 | dot_XI=dot_pos[0]; 1128 | # dot_YI=dot_pos[1]; # do not update longitudinal pos for HV 1129 | 1130 | dot_wR=(Tr-fRx*self.r_R)/self.Iwr; 1131 | 1132 | self.Vx += dot_Vx*dt 1133 | self.Vy += dot_Vy*dt 1134 | self.psi += self.r*dt 1135 | if self.wR + dot_wR*dt<0: 1136 | stop=0 1137 | self.wR += dot_wR*dt 1138 | self.wR = max(1e-1,self.wR) 1139 | 1140 | self.r += dot_r*dt 1141 | self.pos[0] += dot_XI*dt*Resize 1142 | 1143 | self.Vx=bound(self.Vx,self.V_min,self.V_max) 1144 | self.vel = self.Vx+0.0 1145 | 1146 | ##### define controller 1147 | def ORT(self): 1148 | ## Vx_aux = 10.0 1149 | # Vx_aux = max(self.Vx, 5) 1150 | # A=np.matrix([[-(self.Cf+self.Cr)/Vx_aux/self.m,-1+(self.lr*self.Cr-self.lf*self.Cf)/self.m/Vx_aux**2,0,0],[(self.lr*self.Cr-self.lf*self.Cf)/self.Iz,-(self.lr**2*self.Cr+self.lf**2*self.Cf)/self.Iz/Vx_aux,0,0],[-Vx_aux,-self.ls,0,Vx_aux],[0,-1,0,0]]) 1151 | # B=np.matrix([self.Cf/self.m/Vx_aux,self.lf*self.Cf/self.Iz,0,0]).T 1152 | # E=np.matrix([0,0,Vx_aux*self.ls,Vx_aux]).T 1153 | # C=np.matrix([0,0,1,0]) 1154 | # 1155 | # rows = A.shape[0] 1156 | # P = cp.Variable((rows, rows), PSD=True) 1157 | # S = cp.Variable((1,rows)) 1158 | # PI = cp.Variable((rows,1)) 1159 | # 1160 | # Gamma = cp.Variable() 1161 | # epsi = cp.Variable() 1162 | # I = np.identity(rows) 1163 | # 1164 | # cons1 = epsi>=0 1165 | # cons2 = -(A*P+B*S+P.T*A.T+S.T*B.T)>>1e0 1166 | # cons3 = cp.bmat([[-I, A*PI+B*Gamma+E],[PI.T*A.T+Gamma.T*B.T+E.T, -epsi*np.eye(1)]])<<1e-10 1167 | # cons4 = cp.bmat([[-epsi*np.eye(1), C*PI],[PI.T*C.T, -np.identity(1)]])<<1e-10 1168 | # cons5 = P>>1e1 1169 | ## cons6 = epsi-1e-5<=0 1170 | # 1171 | # optprob = cp.Problem(cp.Minimize(epsi), constraints=[cons1, cons2, cons3, cons4, cons5]) 1172 | # result = optprob.solve() 1173 | # 1174 | # aux1 = S.value.dot(np.linalg.pinv(P.value)) 1175 | # aux2 = Gamma.value-aux1.dot(PI.value) 1176 | # self.G = aux1[0] 1177 | # self.H = aux2[0][0] 1178 | 1179 | # self.G = np.array([-0.0260928258891248,0.0152326746505565,0.0468479504998531,0.672122306996471]) 1180 | # self.H = 2.19301870160333 1181 | self.G = np.array([-3.464761346737013525e+00,-7.421571597432523593e-01,1.132624286600968144e+00,4.632126864955187884e+00]) 1182 | self.H = 14.436298476877123 1183 | 1184 | # self.G = np.array([-0.171908282309310,-0.0194559256488496,0.0686712014555834,0.972969665762630]) 1185 | # self.H = 4.46361884075154 1186 | 1187 | ##### implement action 1188 | 1189 | def maintain_fcn(self,car_front,steps=np.int(maintain_Time/dt)): 1190 | if self.in_action: 1191 | self.move_count+=1 1192 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 1193 | 1194 | # yaw_err = self.psi-np.pi/2 1195 | delta_y=self.pos[0]/Resize+self.ls*mh.cos(self.psi)-(Center_x+(self.lane-2)*width)/Resize; 1196 | delta_psi = np.pi/2-self.psi; 1197 | # self.ORT() 1198 | delta = self.G.dot(np.array([ atan(self.Vy/self.Vx), self.r, delta_y, delta_psi ])) # check math 1199 | 1200 | if car_front is not None: 1201 | dist=-(car_front.pos[1]-self.pos[1])/Resize 1202 | 1203 | if car_front==None: 1204 | v_err=self.vel-self.vel_tg 1205 | self.acc = self.nu*v_err 1206 | elif self.vel>=car_front.vel and dist<2*self.grid_len: 1207 | self.vel_tg=car_front.vel 1208 | s_err=-dist+self.grid_len 1209 | v_err=self.vel-self.vel_tg 1210 | self.acc = self.nu1*v_err+self.nu2*s_err 1211 | else: #if self.vel=steps) and yaw_err**2<25e-4: 1220 | if self.move_count>=steps and delta_psi**2<25e-4 and self.r**2<1e-2: 1221 | self.move_count=0 1222 | self.in_action=False 1223 | 1224 | 1225 | def acc_fcn(self,car_front,acc,steps=np.int(maintain_Time/dt)): 1226 | if self.in_action: 1227 | 1228 | # yaw_err = self.psi-np.pi/2 1229 | delta_y=self.pos[0]/Resize+self.ls*mh.cos(self.psi)-(Center_x+(self.lane-2)*width)/Resize; 1230 | delta_psi = np.pi/2-self.psi; 1231 | # self.ORT() 1232 | delta = self.G.dot(np.array([ atan(self.Vy/self.Vx), self.r, delta_y, delta_psi ])) # check math 1233 | 1234 | if acc or car_front==None: 1235 | self.move_count_pure_speeding+=1 1236 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 1237 | self.vel_tg=self.V_max 1238 | v_err=self.vel-self.vel_tg 1239 | self.acc = self.nu*v_err 1240 | else: # provide the car making acc==0 1241 | self.move_count+=1 1242 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 1243 | dist=-(car_front.pos[1]-self.pos[1])/Resize 1244 | self.vel_tg=car_front.vel+0 1245 | s_err=-dist+self.grid_len 1246 | v_err=self.vel-self.vel_tg 1247 | self.acc = self.nu1*v_err+self.nu2*s_err 1248 | 1249 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 1250 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 1251 | self.vehicle_dynamics(delta,Tr_aux,dt) 1252 | # self.vel=bound(self.vel,self.V_min,self.V_max) 1253 | if (v_err**2<0.25 or self.move_count_pure_speeding>=steps or self.move_count>=steps) and delta_psi**2<25e-4 and self.r**2<1e-2: 1254 | self.move_count_pure_speeding=0 1255 | self.move_count=0 1256 | self.in_action=False 1257 | 1258 | def brk_fcn(self,car_front,steps=np.int(maintain_Time/dt)): 1259 | if self.in_action: 1260 | self.move_count+=1 1261 | 1262 | # yaw_err = self.psi-np.pi/2 1263 | delta_y=self.pos[0]/Resize+self.ls*mh.cos(self.psi)-(Center_x+(self.lane-2)*width)/Resize; 1264 | delta_psi = np.pi/2-self.psi; 1265 | # self.ORT() 1266 | delta = self.G.dot(np.array([ atan(self.Vy/self.Vx), self.r, delta_y, delta_psi ])) # check math 1267 | 1268 | if car_front is not None and self.vel>car_front.vel: 1269 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 1270 | dist=-(car_front.pos[1]-self.pos[1])/Resize 1271 | vel_aux=max(self.V_min,min(car_front.vel,self.vel_tg*0.8)) 1272 | s_err=-dist+self.grid_len 1273 | v_err=self.vel-vel_aux 1274 | self.acc = self.nu1*v_err+self.nu2*s_err 1275 | else: 1276 | self.grid_len=self.car_length/Resize+self.vel*self.Dt 1277 | v_err=self.vel-self.vel_tg*0.8 1278 | self.acc = self.nu*v_err 1279 | 1280 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 1281 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 1282 | self.vehicle_dynamics(delta,Tr_aux,dt) 1283 | # self.vel=bound(self.vel,self.V_min,self.V_max) 1284 | if (v_err**2<0.25 or self.move_count>=steps) and delta_psi**2<25e-4 and self.r**2<1e-2: 1285 | self.move_count=0 1286 | self.in_action=False 1287 | 1288 | 1289 | def left_fcn(self,car_front): 1290 | if self.in_action: 1291 | # self.move_count+=1 1292 | # self.pos[0]-=width/steps 1293 | lat_err = self.pos[0]/Resize-self.Curve_AB_Ex_Rotate[-1,0] 1294 | yaw_err = self.psi-np.pi/2 1295 | pos_pred = np.zeros(2) 1296 | pos_pred[0]=self.pos[0]/Resize+self.ls*mh.cos(self.psi) 1297 | pos_pred[1]=(2*self.Curve_AB_Ex_Rotate[0,1]-self.pos[1]/Resize)+self.ls*mh.sin(self.psi) 1298 | travel_dis = pos_pred[1]-self.Curve_AB_Ex_Rotate[0,1] 1299 | if travel_discar_front.vel: 1316 | dist=-(car_front.pos[1]-self.pos[1])/Resize 1317 | self.vel_tg=car_front.vel+0 1318 | s_err=-dist+self.grid_len 1319 | v_err=self.vel-self.vel_tg 1320 | self.acc = self.nu1*v_err+self.nu2*s_err 1321 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 1322 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 1323 | 1324 | self.vehicle_dynamics(delta,Tr_aux,dt) 1325 | # self.vel=bound(self.vel,self.V_min,self.V_max) 1326 | # self.Curve_AB_Ex_Rotate[:,1]+=self.Vx*dt 1327 | self.Curve_AB_Ex_Rotate[:,1]=self.Curve_AB_Ex_Rotate[:,1]+HV.Vx*dt 1328 | if lat_err**2<=1e-2 and yaw_err**2<25e-4 and self.r**2<1e-2:#travel_dis>=extra_dis+self.Length: #lat_err**2<=1e-2: 1329 | # self.move_count=0 1330 | self.in_action=False 1331 | self.lane = self.lane_tg+0 1332 | 1333 | 1334 | def right_fcn(self,car_front): 1335 | if self.in_action: 1336 | lat_err = self.pos[0]/Resize-self.Curve_AB_Ex_Rotate[-1,0] 1337 | yaw_err = self.psi-np.pi/2 1338 | pos_pred = np.zeros(2) 1339 | pos_pred[0]=self.pos[0]/Resize+self.ls*mh.cos(self.psi) 1340 | pos_pred[1]=(2*self.Curve_AB_Ex_Rotate[0,1]-self.pos[1]/Resize)+self.ls*mh.sin(self.psi) 1341 | travel_dis = pos_pred[1]-self.Curve_AB_Ex_Rotate[0,1] 1342 | if travel_discar_front.vel: 1359 | dist=-(car_front.pos[1]-self.pos[1])/Resize 1360 | self.vel_tg=car_front.vel+0 1361 | s_err=-dist+self.grid_len 1362 | v_err=self.vel-self.vel_tg 1363 | self.acc = self.nu1*v_err+self.nu2*s_err 1364 | self.acc = bound( self.acc,self.acc_low,self.acc_up ) 1365 | Tr_aux = self.acc*self.r_R*(self.m+self.Iwr/self.r_R**2) 1366 | 1367 | self.vehicle_dynamics(delta,Tr_aux,dt) 1368 | # self.vel=bound(self.vel,self.V_min,self.V_max) 1369 | # self.Curve_AB_Ex_Rotate[:,1]+=self.Vx*dt 1370 | self.Curve_AB_Ex_Rotate[:,1]=self.Curve_AB_Ex_Rotate[:,1]+HV.Vx*dt 1371 | if lat_err**2<=1e-2 and yaw_err**2<25e-4 and self.r**2<1e-2:#travel_dis>=extra_dis+self.Length: #lat_err**2<=1e-2: 1372 | self.in_action=False 1373 | self.lane = self.lane_tg+0 1374 | 1375 | def TakeAction_HV(self,state,car_front): 1376 | if not self.in_action: 1377 | if state[0] == 1: 1378 | state_row_index = sum( np.array([2**4,2**3,0,2**2,0,2,1,0])*state[1:9] ) 1379 | elif state[0] == 2: 1380 | state_row_index = 2**5+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*state[1:9] ) 1381 | elif state[0] == 3: 1382 | state_row_index = 2**5+2**8+sum( np.array([0,2**4,2**3,0,2**2,0,2,1])*state[1:9] ) 1383 | 1384 | best_action_index = self.Policy[int(round(state_row_index)),self.Curvature_Class].astype(int) 1385 | self.Action = ActionSet[best_action_index] 1386 | self.vel_tg = self.vel+0.0 1387 | self.in_action = True 1388 | 1389 | if self.Action=='left' or self.Action=='right': 1390 | if self.Action=='left': 1391 | direction = 1.0 1392 | else: 1393 | direction = -1.0 1394 | k_max = k_max = min(1.0/150, 0.25*self.D*g/self.Vx**2) 1395 | self.tau_ex, self.Curve_AB_Ex_Rotate, self.ka_Ex, self.psi_t, self.Length = path_planning(self.pos,k_max,width/Resize,direction) 1396 | 1397 | self.ORT() 1398 | 1399 | acc,brk,lf,rt=Safe_actions_HV(state) 1400 | 1401 | if self.Action=='stay': 1402 | self.maintain_fcn(car_front) 1403 | elif self.Action=='acc': 1404 | self.acc_fcn(car_front,acc) 1405 | elif self.Action=='brk': 1406 | self.brk_fcn(car_front) 1407 | elif self.Action=='left': 1408 | self.lane_tg = self.lane-1 1409 | self.left_fcn(car_front) 1410 | else: 1411 | self.lane_tg = self.lane+1 1412 | self.right_fcn(car_front) 1413 | 1414 | 1415 | #=============================================== 1416 | 1417 | pygame.init() 1418 | 1419 | gameDisplay = pygame.display.set_mode((display_width,display_height),pygame.RESIZABLE) 1420 | pygame.display.set_caption('Multi lane animation') 1421 | 1422 | black = (0,0,0) 1423 | white = (255,255,255) 1424 | 1425 | clock = pygame.time.Clock() 1426 | crashed = False 1427 | 1428 | myfont = pygame.font.SysFont('Comic Sans MS', 50) 1429 | 1430 | GreenCar_HV = pygame.image.load('ImageFolder/GreenCar_HV.png') 1431 | GreenCar = pygame.image.load('ImageFolder/GreenCar.png') 1432 | BlueCar = pygame.image.load('ImageFolder/BlueCar.png') 1433 | PinkCar = pygame.image.load('ImageFolder/PinkCar.png') 1434 | GreyCar = pygame.image.load('ImageFolder/GreyCar.png') 1435 | PurpleCar = pygame.image.load('ImageFolder/PurpleCar.png') 1436 | RedCar = pygame.image.load('ImageFolder/RedCar.png') 1437 | YellowCar = pygame.image.load('ImageFolder/YellowCar.png') 1438 | 1439 | GreenTruck = pygame.image.load('ImageFolder/GreenTruck.png') 1440 | BlueTruck = pygame.image.load('ImageFolder/BlueTruck.png') 1441 | GreyTruck = pygame.image.load('ImageFolder/GreyTruck.png') 1442 | RedTruck = pygame.image.load('ImageFolder/RedTruck.png') 1443 | YellowTruck = pygame.image.load('ImageFolder/YellowTruck.png') 1444 | 1445 | 1446 | Straight = pygame.image.load('ImageFolder/straight.png') 1447 | Circuit = pygame.image.load('ImageFolder/circuit1.png') # need png, rotation keeps trnasparency 1448 | Circuit = pygame.image.load('ImageFolder/circuit3.png') 1449 | 1450 | LeftTurn = pygame.image.load('ImageFolder/LeftTurn3.png') 1451 | RightTurn = pygame.image.load('ImageFolder/RightTurn3.png') 1452 | 1453 | def green_car_HV(x,y,car_width,car_length,car_angle=0): 1454 | GreenCar_HV_rota = pygame.transform.rotate(GreenCar_HV,car_angle) 1455 | gameDisplay.blit(GreenCar_HV_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1456 | 1457 | def green_car(x,y,car_width,car_length,car_angle=0): 1458 | GreenCar_rota = pygame.transform.rotate(GreenCar,car_angle) 1459 | gameDisplay.blit(GreenCar_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1460 | def blue_car(x,y,car_width,car_length,car_angle=0): 1461 | BlueCar_rota = pygame.transform.rotate(BlueCar,car_angle) 1462 | gameDisplay.blit(BlueCar_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1463 | def pink_car(x,y,car_width,car_length,car_angle=0): 1464 | PinkCar_rota = pygame.transform.rotate(PinkCar,car_angle) 1465 | gameDisplay.blit(PinkCar_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1466 | def grey_car(x,y,car_width,car_length,car_angle=0): 1467 | GreyCar_rota = pygame.transform.rotate(GreyCar,car_angle) 1468 | gameDisplay.blit(GreyCar_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1469 | def purple_car(x,y,car_width,car_length,car_angle=0): 1470 | PurpleCar_rota = pygame.transform.rotate(PurpleCar,car_angle) 1471 | gameDisplay.blit(PurpleCar_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1472 | def red_car(x,y,car_width,car_length,car_angle=0): 1473 | RedCar_rota = pygame.transform.rotate(RedCar,car_angle) 1474 | gameDisplay.blit(RedCar_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1475 | def yellow_car(x,y,car_width,car_length,car_angle=0): 1476 | YellowCar_rota = pygame.transform.rotate(YellowCar,car_angle) 1477 | gameDisplay.blit(YellowCar_rota, (x-car_width/2*np.cos(car_angle/180*np.pi)-car_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-car_length/2*np.cos(car_angle/180*np.pi))-car_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1478 | 1479 | def green_truck(x,y,truck_width,truck_length,car_angle=0): 1480 | GreenTruck_rota = pygame.transform.rotate(GreenTruck,car_angle) 1481 | gameDisplay.blit(GreenTruck_rota, (x-truck_width/2*np.cos(car_angle/180*np.pi)-truck_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-truck_length/2*np.cos(car_angle/180*np.pi))-truck_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1482 | def blue_truck(x,y,truck_width,truck_length,car_angle=0): 1483 | BlueTruck_rota = pygame.transform.rotate(BlueTruck,car_angle) 1484 | gameDisplay.blit(BlueTruck_rota, (x-truck_width/2*np.cos(car_angle/180*np.pi)-truck_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-truck_length/2*np.cos(car_angle/180*np.pi))-truck_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1485 | def grey_truck(x,y,truck_width,truck_length,car_angle=0): 1486 | GreyTruck_rota = pygame.transform.rotate(GreyTruck,car_angle) 1487 | gameDisplay.blit(GreyTruck_rota, (x-truck_width/2*np.cos(car_angle/180*np.pi)-truck_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-truck_length/2*np.cos(car_angle/180*np.pi))-truck_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1488 | def red_truck(x,y,truck_width,truck_length,car_angle=0): 1489 | RedTruck_rota = pygame.transform.rotate(RedTruck,car_angle) 1490 | gameDisplay.blit(RedTruck_rota, (x-truck_width/2*np.cos(car_angle/180*np.pi)-truck_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-truck_length/2*np.cos(car_angle/180*np.pi))-truck_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1491 | def yellow_truck(x,y,truck_width,truck_length,car_angle=0): 1492 | YellowTruck_rota = pygame.transform.rotate(YellowTruck,car_angle) 1493 | gameDisplay.blit(YellowTruck_rota, (x-truck_width/2*np.cos(car_angle/180*np.pi)-truck_length/2*np.sin(np.abs(car_angle/180*np.pi)),y-truck_length/2*np.cos(car_angle/180*np.pi))-truck_width/2*np.sin(np.abs(car_angle/180*np.pi))) 1494 | 1495 | def straight(x,y,width,length,road_angle=0): # left: angle>0 right:angle<0, (x,y): bottom line center 1496 | Straight_rota = pygame.transform.rotate(Straight,road_angle) 1497 | if road_angle>=0: 1498 | gameDisplay.blit(Straight_rota, (x-width/2*np.cos(road_angle/180*np.pi)-length*np.sin(road_angle/180*np.pi),y-length*np.cos(road_angle/180*np.pi)-width/2*np.sin(road_angle/180*np.pi))) 1499 | else: 1500 | gameDisplay.blit(Straight_rota, (x-width/2*np.cos(-road_angle/180*np.pi),y-length*np.cos(-road_angle/180*np.pi)-width/2*np.sin(-road_angle/180*np.pi))) 1501 | 1502 | def circuit(x,y,circuit_edge,circuit_angle=0): # left: angle>0 right:angle<0, (x,y): quadrant circuit center 1503 | if circuit_angle>=0: 1504 | angle_aux = circuit_angle%90 + 0 1505 | Circuit_rota1 = pygame.transform.rotate(Circuit,angle_aux) 1506 | Circuit_rota2 = pygame.transform.rotate(Circuit,-(90-angle_aux)+180) 1507 | gameDisplay.blit(Circuit_rota1, (x-circuit_edge*np.sin(angle_aux/180*np.pi),y-circuit_edge*np.cos(angle_aux/180*np.pi)-circuit_edge*np.sin(angle_aux/180*np.pi))) 1508 | gameDisplay.blit(Circuit_rota2, (x-circuit_edge*np.cos(angle_aux/180*np.pi)-circuit_edge*np.sin(angle_aux/180*np.pi),y-circuit_edge*np.cos(angle_aux/180*np.pi))) 1509 | else: 1510 | angle_aux = circuit_angle%90 + 0 1511 | Circuit_rota1 = pygame.transform.rotate(Circuit,angle_aux) 1512 | Circuit_rota2 = pygame.transform.rotate(Circuit,-(90-angle_aux)) 1513 | gameDisplay.blit(Circuit_rota1, (x-circuit_edge*np.sin(angle_aux/180*np.pi),y-circuit_edge*np.cos(angle_aux/180*np.pi)-circuit_edge*np.sin(angle_aux/180*np.pi))) 1514 | gameDisplay.blit(Circuit_rota2, (x,y-circuit_edge*np.sin(angle_aux/180*np.pi))) 1515 | 1516 | def left_turn(x,y): 1517 | gameDisplay.blit(LeftTurn, (x,y)) 1518 | def right_turn(x,y): 1519 | gameDisplay.blit(RightTurn, (x,y)) 1520 | 1521 | Car_dict = {'GreenCar':green_car,'BlueCar':blue_car,'PinkCar':pink_car,'GreyCar':grey_car,'PurpleCar':purple_car,'RedCar':red_car,'YellowCar':yellow_car,'GreenTruck':green_truck,'BlueTruck':blue_truck,'GreyTruck':grey_truck,'RedTruck':red_truck,'YellowTruck':yellow_truck} 1522 | Car_dict_HV = {'GreenCar_HV':green_car_HV} 1523 | 1524 | dx = -2 1525 | dy = 0 1526 | Road_x0 = display_width * 0.5 1527 | Road_y0 = display_height #* 0.5 1528 | 1529 | circuit_x = display_width-circuit_edge 1530 | circuit_y = display_height 1531 | 1532 | Center_x = display_width * 0.5 + dx 1533 | Center_y = display_height * 0.5 + dy 1534 | 1535 | #===================================================================== 1536 | Initial_state = [2,1,0,1,0,0,0,0,1 ] 1537 | Initial_state_matrix = state2matrix(Initial_state) 1538 | aux0 = np.concatenate(([0],Initial_state_matrix[0,:],[0])) 1539 | aux1 = np.concatenate(([0],Initial_state_matrix[1,:],[0])) 1540 | aux2 = np.concatenate(([0],Initial_state_matrix[2,:],[0])) 1541 | Initial_big_state_matrix = np.array([[1,0,0,0,0],aux0,aux1,aux2,[0,0,0,1,0]]) 1542 | 1543 | Initial_HV_lane = NrLanes-3 # HV_lane range from 0-4 1544 | Nr_action = 5 # total number of actions 1545 | 1546 | Find_EV_indices = np.asarray(np.where(Initial_big_state_matrix == 1 )).T 1547 | Initial_EV = len(Find_EV_indices) # total number of EVs 1548 | EV_name_set = set(np.array(range(Initial_EV))+1) # name list of active EVs, start from 1, reserve 0 for HV (faked EV) 1549 | EV_pos_list = Find_EV_indices 1550 | # epsilon = 1e-10 1551 | 1552 | #========================= Plot Initial Figure ================================ 1553 | straight(Road_x0,Road_y0,road_width,road_length,0) 1554 | 1555 | Curvature_Class = 0 1556 | HV_color_index = 0 1557 | Policy = np.loadtxt('save_optimal_policy.txt') 1558 | #Policy = np.loadtxt('save_optimal_policy_tailgating.txt') 1559 | Pos=np.array([Center_x,Center_y]) 1560 | Vel=10.0 1561 | Dt=1.2 1562 | HV = HV_obj(Initial_HV_lane, Pos, Vel, Dt, Policy, HV_color_index, Curvature_Class) 1563 | Car_dict_HV[HV.Color](Center_x,Center_y,HV.car_width,HV.car_length) 1564 | 1565 | PositionTable = np.zeros((5,5,2)) 1566 | for i in range(5): 1567 | for j in range(5): 1568 | PositionTable[i,j,0] = Center_x+(j-2)*width 1569 | PositionTable[i,j,1] = Center_y+(i-2)*HV.grid_len*Resize 1570 | 1571 | EV_set={} 1572 | EV_set["EV{}".format(0)]=HV 1573 | 1574 | i=0 1575 | for x in EV_name_set: 1576 | EV_pos = np.array(Find_EV_indices[i]) 1577 | Pos = PositionTable[EV_pos[0],EV_pos[1],:] 1578 | EV_color_index = np.random.randint(0, high=len(ColorSet)) 1579 | EV_lane=EV_pos[1] 1580 | Vel=12.0 1581 | Dt=1.2 1582 | EV_set["EV{}".format(x)]=EV(EV_lane, Pos, Vel, Dt, EV_color_index, Curvature_Class) 1583 | Car_dict[EV_set["EV{}".format(x)].Color](Pos[0],Pos[1],EV_set["EV{}".format(x)].car_width,EV_set["EV{}".format(x)].car_length) 1584 | i+=1 1585 | 1586 | pygame.display.update() 1587 | clock.tick(20) 1588 | 1589 | #============================================================================== 1590 | 1591 | state = Initial_state 1592 | 1593 | step = 0 1594 | plot_after_time = 0.1 #sec 1595 | plot_after_steps = int(plot_after_time/dt) 1596 | angle_scale = 1 1597 | 1598 | x_left = display_width * 0.01 1599 | y_left = display_height * 0.45 1600 | 1601 | x_right = display_width * 0.855 1602 | y_right = display_height * 0.45 1603 | 1604 | Curve_Last_Steps = 300*plot_after_steps 1605 | 1606 | save_screen = make_video(gameDisplay) # initiate the video generator 1607 | video = False # at start: video not active 1608 | plot_straight = True 1609 | plot_straigt2circuit = False 1610 | plot_circuit = False 1611 | plot_circuit2straight = False 1612 | 1613 | count = 0 1614 | travel_dist_aux=0 1615 | road_x_transit = 0 1616 | road_y_transit = 0 1617 | road_angle_transit = 0 1618 | circuit_x = 0 1619 | circuit_y = 0 1620 | 1621 | road_x_offset1 = 11 # corner to straigt transition error 1622 | road_x_offset2 = 10 1623 | 1624 | circuit_x_offset1 = 110 #10 # straight to corner transition error 1625 | circuit_y_offset1 = 0 1626 | circuit_x_offset2 = 120 #20 1627 | circuit_y_offset2 = 0 1628 | 1629 | while not crashed: 1630 | for event in pygame.event.get(): 1631 | if event.type == pygame.QUIT: 1632 | crashed = True 1633 | elif event.type == VIDEORESIZE: 1634 | gameDisplay = pygame.display.set_mode((event.w, event.h),pygame.RESIZABLE) 1635 | # toggle video on/off by clicking 'v' on keyboard # 1636 | elif event.type == pygame.KEYDOWN and event.key == pygame.K_v: 1637 | video = not video 1638 | 1639 | # update curvature first 1640 | Curvature_Class_aux = (step//Curve_Last_Steps)%4 1641 | if Curvature_Class_aux == 3: 1642 | HV.Curvature_Class = 1 1643 | else: 1644 | HV.Curvature_Class = Curvature_Class_aux+0 1645 | 1646 | if plot_straight == True: 1647 | travel_dist_aux+=HV.vel*dt*Resize 1648 | modeNr = travel_dist_aux//road_length 1649 | if step%plot_after_steps==0: 1650 | straight(Road_x0,Road_y0-road_length*modeNr+travel_dist_aux,road_width,road_length) 1651 | straight(Road_x0,Road_y0-road_length*(modeNr+1)+travel_dist_aux,road_width,road_length) 1652 | 1653 | if HV.Curvature_Class != 1: 1654 | plot_straight = False 1655 | plot_straigt2circuit = True 1656 | residue_travel = travel_dist_aux%road_length-road_margin #position of window top related to road connection line 1657 | travel_dist_aux=0 1658 | 1659 | if plot_straigt2circuit == True and plot_straight == False: 1660 | if residue_travel <= 0: 1661 | if HV.Curvature_Class == 0: 1662 | circuit_x = display_width-circuit_edge+circuit_x_offset1 1663 | angle = -1e-2 1664 | elif HV.Curvature_Class == 2: 1665 | circuit_x = circuit_edge-circuit_x_offset2 1666 | angle = 90-1e-2 #make show possible 1667 | circuit_y = residue_travel+travel_dist_aux 1668 | travel_dist_aux+=HV.vel*dt*Resize 1669 | if circuit_y >= display_height: 1670 | circuit_y = display_height+0 1671 | plot_straigt2circuit = False 1672 | plot_circuit = True 1673 | travel_dist_aux=0 1674 | if step%plot_after_steps==0: 1675 | circuit(circuit_x,circuit_y,circuit_edge,angle) 1676 | straight(Road_x0,circuit_y+road_length,road_width,road_length) 1677 | elif residue_travel > 0: 1678 | if HV.Curvature_Class == 0: 1679 | circuit_x = display_width-circuit_edge+circuit_x_offset1 1680 | angle = -1e-2 1681 | elif HV.Curvature_Class == 2: 1682 | circuit_x = circuit_edge-circuit_x_offset2 1683 | angle = 90-1e-2 #make show possible 1684 | circuit_y = -(road_length-residue_travel)+travel_dist_aux #vel_step*count 1685 | travel_dist_aux+=HV.vel*dt*Resize #count = count+1 1686 | if circuit_y >= display_height: 1687 | circuit_y = display_height+0 1688 | plot_straigt2circuit = False 1689 | plot_circuit = True 1690 | travel_dist_aux=0 1691 | if step%plot_after_steps==0: 1692 | circuit(circuit_x,circuit_y,circuit_edge,angle) 1693 | straight(Road_x0,circuit_y+road_length,road_width,road_length) 1694 | straight(Road_x0,circuit_y+2*road_length,road_width,road_length) 1695 | angle = 0 1696 | 1697 | if plot_straigt2circuit == False and plot_circuit == True: 1698 | if HV.Curvature_Class == 0: 1699 | circuit_x = display_width-circuit_edge+circuit_x_offset1 1700 | angle = angle-HV.vel*dt*Resize/R3/np.pi*180*angle_scale 1701 | elif HV.Curvature_Class == 2: 1702 | circuit_x = circuit_edge-circuit_x_offset2 1703 | angle = angle+HV.vel*dt*Resize/R3/np.pi*180*angle_scale 1704 | circuit_y = display_height 1705 | 1706 | if step%plot_after_steps==0: 1707 | circuit(circuit_x,circuit_y,circuit_edge,angle) 1708 | 1709 | if HV.Curvature_Class == 1: 1710 | plot_circuit = False 1711 | plot_circuit2straight = True 1712 | phi = np.arcsin(display_height/R3)/np.pi*180 1713 | angle_pre_transit = angle+0 # circuit angle 1714 | phi_transit = phi+0 1715 | 1716 | if plot_circuit == False and plot_circuit2straight == True: 1717 | circuit_y = display_height 1718 | if count != 0: 1719 | phi_transit = phi_transit-HV.vel*dt*Resize/R3/np.pi*180*angle_scale 1720 | if angle_pre_transit<0: # CW, left turn 1721 | circuit_x = display_width-circuit_edge+circuit_x_offset1 1722 | angle = angle-HV.vel*dt*Resize/R3/np.pi*180*angle_scale 1723 | count = count+1 1724 | if angle-angle_pre_transit<=-phi: 1725 | angle = angle_pre_transit-phi 1726 | phi_transit = 0 1727 | count = 0 1728 | plot_circuit2straight = False 1729 | plot_straight = True 1730 | road_x_transit = circuit_x+R3*mh.cos(phi_transit/180*np.pi)+road_x_offset1 1731 | road_angle_transit = phi_transit+0 1732 | 1733 | elif angle>0: # CCW, right turn 1734 | circuit_x = circuit_edge-circuit_x_offset2 1735 | angle = angle+HV.vel*dt*Resize/R3/np.pi*180*angle_scale 1736 | count = count+1 1737 | if angle-angle_pre_transit>=phi: 1738 | angle = angle_pre_transit+phi 1739 | phi_transit = 0 1740 | count = 0 1741 | plot_circuit2straight = False 1742 | plot_straight = True 1743 | road_x_transit = circuit_x-R3*mh.cos(phi_transit/180*np.pi)-road_x_offset2 1744 | road_angle_transit = -phi_transit+0 1745 | 1746 | road_y_transit = display_height-R3*mh.sin(phi_transit/180*np.pi) 1747 | if step%plot_after_steps==0: 1748 | circuit(circuit_x,circuit_y,circuit_edge,angle) 1749 | straight(road_x_transit,road_y_transit,road_width,road_length,road_angle_transit) 1750 | 1751 | state_HV,car_front=get_state_HV(EV_set,EV_name_set,HV) 1752 | HV.TakeAction_HV(state_HV,car_front) 1753 | if step%plot_after_steps==0: 1754 | x_aux2,y_aux2,angle_aux2 = mapping2curve(plot_straight,plot_straigt2circuit,plot_circuit,plot_circuit2straight,HV.pos[0],HV.pos[1],circuit_x,circuit_y,HV.Curvature_Class,road_y_transit,road_angle_transit,R1,R2,R3,R4,R5,display_width,display_height) 1755 | Car_dict_HV[HV.Color](x_aux2,y_aux2,HV.car_width,HV.car_length,angle_aux2+HV.psi/np.pi*180-90) 1756 | 1757 | EV_name_set0 = EV_name_set|set() 1758 | for x in EV_name_set: 1759 | EV_name_set_aux = EV_name_set|set() 1760 | EV_name_set_aux.remove(x) 1761 | EV_name_set_aux.add(0) # introduce HV 1762 | VehicleArray_EV,car_front=get_state_EV(EV_set,EV_name_set_aux,EV_set["EV{}".format(x)]) 1763 | EV_set["EV{}".format(x)].TakeAction(VehicleArray_EV,car_front,HV) 1764 | x_aux=EV_set["EV{}".format(x)].pos[0] 1765 | y_aux=EV_set["EV{}".format(x)].pos[1] 1766 | if step%plot_after_steps==0: 1767 | x_aux2,y_aux2,angle_aux2 = mapping2curve(plot_straight,plot_straigt2circuit,plot_circuit,plot_circuit2straight,x_aux,y_aux,circuit_x,circuit_y,HV.Curvature_Class,road_y_transit,road_angle_transit,R1,R2,R3,R4,R5,display_width,display_height) 1768 | angle_aux3=angle_aux2+EV_set["EV{}".format(x)].psi/np.pi*180-90 1769 | Car_dict[EV_set["EV{}".format(x)].Color](x_aux2,y_aux2,EV_set["EV{}".format(x)].car_width,EV_set["EV{}".format(x)].car_length,angle_aux3) 1770 | y_low=-EV_set["EV{}".format(x)].grid_len*Resize-EV_set["EV{}".format(x)].car_length/2.0 1771 | y_up=display_height+EV_set["EV{}".format(x)].grid_len*Resize+EV_set["EV{}".format(x)].car_length/2.0 1772 | if y_auxy_up: 1773 | EV_name_set0.remove(x) 1774 | EV_name_set=EV_name_set0|set() 1775 | 1776 | if step%Period==0: 1777 | EV_set,EV_name_set=add_EVs(EV_set,EV_name_set,HV) 1778 | ### end ### HV takes actions and plot new vehicles positions ########## 1779 | 1780 | ############################## add road signs begin ########################### 1781 | if step%plot_after_steps==0: 1782 | if HV.Curvature_Class == 0: 1783 | left_turn(x_right,y_right) 1784 | elif HV.Curvature_Class == 2: 1785 | right_turn(x_left,y_left) 1786 | 1787 | ############################## add road signs end ############################# 1788 | textsurface = myfont.render('Speed {} km/h'.format(round(HV.vel*3.6,2)), False, (0, 0, 0)) 1789 | if HV.Curvature_Class == 0: 1790 | gameDisplay.blit(textsurface,(500,20)) 1791 | else: 1792 | gameDisplay.blit(textsurface,(10,20)) 1793 | ############################################################################### 1794 | pygame.display.update() 1795 | print('Simulated step: {}'.format(step)) 1796 | 1797 | clock.tick(30) 1798 | if video: 1799 | next(save_screen) # call the generator 1800 | 1801 | step = step + 1 1802 | 1803 | 1804 | pygame.quit() 1805 | quit() 1806 | 1807 | -------------------------------------------------------------------------------- /ImageFolder/BlueCar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/BlueCar.png -------------------------------------------------------------------------------- /ImageFolder/BlueTruck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/BlueTruck.png -------------------------------------------------------------------------------- /ImageFolder/GreenCar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/GreenCar.png -------------------------------------------------------------------------------- /ImageFolder/GreenCar_HV.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/GreenCar_HV.png -------------------------------------------------------------------------------- /ImageFolder/GreenTruck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/GreenTruck.png -------------------------------------------------------------------------------- /ImageFolder/GreyCar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/GreyCar.png -------------------------------------------------------------------------------- /ImageFolder/GreyTruck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/GreyTruck.png -------------------------------------------------------------------------------- /ImageFolder/LeftTurn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/LeftTurn.png -------------------------------------------------------------------------------- /ImageFolder/LeftTurn2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/LeftTurn2.png -------------------------------------------------------------------------------- /ImageFolder/LeftTurn3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/LeftTurn3.png -------------------------------------------------------------------------------- /ImageFolder/PinkCar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/PinkCar.png -------------------------------------------------------------------------------- /ImageFolder/PurpleCar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/PurpleCar.png -------------------------------------------------------------------------------- /ImageFolder/RedCar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/RedCar.png -------------------------------------------------------------------------------- /ImageFolder/RedTruck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/RedTruck.png -------------------------------------------------------------------------------- /ImageFolder/RightTurn3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/RightTurn3.png -------------------------------------------------------------------------------- /ImageFolder/Road.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/Road.png -------------------------------------------------------------------------------- /ImageFolder/YellowCar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/YellowCar.png -------------------------------------------------------------------------------- /ImageFolder/YellowTruck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/YellowTruck.png -------------------------------------------------------------------------------- /ImageFolder/circuit - Copy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit - Copy.png -------------------------------------------------------------------------------- /ImageFolder/circuit.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit.jpg -------------------------------------------------------------------------------- /ImageFolder/circuit.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit.png -------------------------------------------------------------------------------- /ImageFolder/circuit1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit1.png -------------------------------------------------------------------------------- /ImageFolder/circuit2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit2.jpg -------------------------------------------------------------------------------- /ImageFolder/circuit2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit2.png -------------------------------------------------------------------------------- /ImageFolder/circuit3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit3.png -------------------------------------------------------------------------------- /ImageFolder/circuit_PS.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/circuit_PS.png -------------------------------------------------------------------------------- /ImageFolder/largedash.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/largedash.png -------------------------------------------------------------------------------- /ImageFolder/straight.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/straight.jpg -------------------------------------------------------------------------------- /ImageFolder/straight.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DCSLgatech/Advanced-Planning-Using-RL/0a38dfb2a47f6eb52014cc2a0b806c3457ebbe5d/ImageFolder/straight.png -------------------------------------------------------------------------------- /MaxEntDeepIRL_SingleStep.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Jun 22 12:35:54 2017 4 | 5 | @author: cyou7 6 | """ 7 | 8 | import numpy as np 9 | import random as rd 10 | 11 | #============================================================================== 12 | def sigmoid(x): 13 | return 1.0/(1.0 + np.exp(-x)) 14 | 15 | def sigmoid_deriv(x): # x is layer output 16 | # return sigmoid(x)*(1.0-sigmoid(x)) 17 | return x*(1.0-x) 18 | 19 | def tanh(x): 20 | return np.tanh(x) 21 | 22 | def tanh_deriv(x): # x is layer output 23 | return 1.0 - x**2 24 | 25 | #============================================================================== 26 | # from state vector to vehicle array 27 | def state2matrix(state_aux): # state_aux: 1~3 for HV + binary[1-8] for EV 28 | VehicleArray =np.zeros((3,3)) 29 | 30 | VehicleArray[0,:] = state_aux[1:4] 31 | VehicleArray[2,:] = state_aux[6:9] 32 | if state_aux[0] == 1: 33 | VehicleArray[1,0] = 2 # distinguish from binary value, indicate HV 34 | VehicleArray[1,1:3] = state_aux[4:6] 35 | elif state_aux[0] == 3: 36 | VehicleArray[1,2] = 2 37 | VehicleArray[1,0:2] = state_aux[4:6] 38 | elif state_aux[0] == 2: 39 | VehicleArray[1,1] = 2 40 | VehicleArray[1,0] = state_aux[4] 41 | VehicleArray[1,2] = state_aux[5] 42 | return VehicleArray 43 | 44 | #============================================================================== 45 | # from vehicle array to state vector 46 | def matrix2state(VehicleArray_aux): 47 | 48 | state = np.zeros(9) 49 | state[1:4] = VehicleArray_aux[0,:] 50 | state[6:9] = VehicleArray_aux[2,:] 51 | 52 | indices = np.asarray(np.where(VehicleArray_aux == 2 )).T # get position of HV 53 | 54 | if indices[0][1] == 0: 55 | state[0] = 1 56 | state[4:6] = VehicleArray_aux[1,1:3] 57 | elif indices[0][1] == 1: 58 | state[0] = 2 59 | state[4] = VehicleArray_aux[1,0] 60 | state[5] = VehicleArray_aux[1,2] 61 | elif indices[0][1] == 2: 62 | state[0] = 3 63 | state[4:6] = VehicleArray_aux[1,0:2] 64 | 65 | return state 66 | 67 | #============================================================================== 68 | # available actions of each EV 69 | def Available_actions_EV(big_state_matrix,big_pos,HV_lane,NrLanes): # HV_lane=0 <=> state_aux[0] = 1, HV_lane=right lane <=> state_aux[0] = 3 70 | 71 | aux0 = np.concatenate(([1],big_state_matrix[0,:],[1])) 72 | aux1 = np.concatenate(([1],big_state_matrix[1,:],[1])) 73 | aux2 = np.concatenate(([1],big_state_matrix[2,:],[1])) 74 | aux3 = np.concatenate(([1],big_state_matrix[3,:],[1])) 75 | aux4 = np.concatenate(([1],big_state_matrix[4,:],[1])) 76 | VehicleArray_aux = np.array([[1,0,0,0,0,0,1],aux0,aux1,aux2,aux3,aux4,[1,0,0,0,0,0,1]]) 77 | 78 | pos_aux = big_pos+[1,1] # new position in 7x7 array 79 | pos_aux_acc = pos_aux + [-1,0] 80 | if VehicleArray_aux[pos_aux_acc[0],pos_aux_acc[1]] == 0: 81 | acc = 1 82 | else: 83 | acc = 0 84 | pos_aux_brk = pos_aux + [1,0] 85 | if VehicleArray_aux[pos_aux_brk[0],pos_aux_brk[1]] == 0: 86 | brk = 1 87 | else: 88 | brk = 0 89 | pos_aux_left = pos_aux + [0,-1] 90 | if VehicleArray_aux[pos_aux_left[0],pos_aux_left[1]] == 0: 91 | left = 1 92 | else: 93 | left = 0 94 | 95 | pos_aux_right = pos_aux + [0,1] 96 | if VehicleArray_aux[pos_aux_right[0],pos_aux_right[1]] == 0: 97 | right = 1 98 | else: 99 | right = 0 100 | 101 | return acc, brk, left, right 102 | 103 | #============================================================================== 104 | #============================================================================== 105 | # add new vehicles randomly to vacant grids after one-step motion of HV 106 | # HV_lane has been changed in object HV, by HV.TakeAction() 107 | 108 | def Add_Vehicle_HV_move(state_aux,big_state_matrix,HV_lane,HV_action,NrLanes,EV_name_list,EV_pos_list): # to use only if no collision 109 | 110 | VehicleArray = state2matrix(state_aux) # before HV moves 111 | indices = np.asarray(np.where(big_state_matrix == 2 )).T # in 5x5 array 112 | 113 | EV_name_list_first = [] 114 | EV_name_list_last = [] 115 | EV_pos_row_first = [] # rows of EV_pos_list that are in first row of big_state_matrix 116 | EV_pos_row_last = [] # rows of EV_pos_list that are in last row of big_state_matrix 117 | for m in range(len(EV_name_list)): 118 | if EV_pos_list[m,0] == 0: 119 | EV_name_list_first = EV_name_list_first + [EV_name_list[m]] 120 | EV_pos_row_first = EV_pos_row_first + [m] 121 | elif EV_pos_list[m,0] == 4: 122 | EV_name_list_last = EV_name_list_last + [EV_name_list[m]] 123 | EV_pos_row_last = EV_pos_row_last + [m] 124 | # 125 | # CurEVs = len(indices_EV) 126 | new_EV_order = [-1] # indicate no new EV by default 127 | add_EV_pos_list = np.array([-1,-1]) 128 | 129 | ############################################################################### 130 | # need see how HV_lane effects state 131 | ############################################################################### 132 | 133 | if HV_action == 'left' and HV_lane==0: #change HV to left boundary 134 | state_aux[0] = state_aux[0]-1 135 | state_aux[3] = 0 136 | state_aux[5] = 0 137 | state_aux[8] = 0 #3rd column disappears -> 0 138 | VehicleArray = state2matrix(state_aux) 139 | big_state_matrix[2,1] = 0 140 | big_state_matrix[2,0] = 2 141 | 142 | elif HV_action == 'left' and HV_lane!=0: 143 | big_state_matrix[2,HV_lane+1] = 0 144 | big_state_matrix[2,HV_lane] = 2 145 | VehicleArray = big_state_matrix[1:NrLanes-1,HV_lane-1:HV_lane+2] 146 | 147 | elif HV_action == 'right' and HV_lane==NrLanes-1: # only change HV 148 | state_aux[0] = state_aux[0]+1 149 | state_aux[1] = 0 150 | state_aux[4] = 0 151 | state_aux[6] = 0 # 1st column disappears ->0 152 | VehicleArray = state2matrix(state_aux) 153 | big_state_matrix[2,NrLanes-2] = 0 154 | big_state_matrix[2,NrLanes-1] = 2 155 | 156 | elif HV_action == 'right' and HV_lane!=NrLanes-1: 157 | big_state_matrix[2,HV_lane-1] = 0 158 | big_state_matrix[2,HV_lane] = 2 159 | VehicleArray = big_state_matrix[1:NrLanes-1,HV_lane-1:HV_lane+2] 160 | 161 | elif HV_action == 'acc': 162 | # new_vehicle_Nr = rd.sample(range(NrLanes+1), 1) # maximum nr = NrLanes, full of EVs 163 | new_vehicle_Nr = rd.sample(range(NrLanes), 1) # not full of EVs, up t0 NrLanes-1 EVs 164 | new_vehicle_pos = rd.sample(range(NrLanes), new_vehicle_Nr[0]) 165 | 166 | big_state_matrix[2,indices[0][1]] = 0 167 | big_state_matrix[1,indices[0][1]] = 2 168 | 169 | aux = np.array([[0,0,0,0,0]]) 170 | if len(EV_name_list)==0: 171 | max_EV_order = 0 172 | else: 173 | max_EV_order = max(EV_name_list) 174 | # EV_name_list = EV_name_list[0:CurEVs-Nr_EV_last] # remove last row EV orders 175 | # EV_pos_list = EV_pos_list[0:CurEVs-Nr_EV_last,:] 176 | EV_name_list = [item for item in EV_name_list if item not in EV_name_list_last] 177 | EV_pos_list = np.delete(EV_pos_list, (EV_pos_row_last), axis=0) 178 | 179 | if new_vehicle_Nr[0] == 0: 180 | big_state_matrix = np.concatenate((aux,big_state_matrix[0:4,:])) 181 | EV_pos_list = EV_pos_list + [1,0] 182 | else: 183 | EV_pos_list = EV_pos_list + [1,0] 184 | add_EV_pos_list = np.array([], dtype=np.int64).reshape(0,2) 185 | k = 0 186 | new_EV_order = [] 187 | for i in new_vehicle_pos: 188 | k = k+1 189 | aux[0][i] = 1 190 | EV_name_list = [max_EV_order+k] + EV_name_list # append new EV orders to the front 191 | EV_pos_list = np.concatenate((np.array([[0,i]]),EV_pos_list)) 192 | add_EV_pos_list = np.concatenate((np.array([[0,i]]),add_EV_pos_list)) # added EVs' positions 193 | new_EV_order = [max_EV_order+k] + new_EV_order 194 | # new_EV_order = list(np.asarray(new_vehicle_pos) + max_EV_order) 195 | big_state_matrix = np.concatenate((aux,big_state_matrix[0:4,:])) # assume only to show 5 rows of vehicles 196 | 197 | VehicleArray = np.zeros((3,3)) 198 | if HV_lane == 0: 199 | VehicleArray[:,0:2] = big_state_matrix[1:-1,0:2] 200 | elif HV_lane==NrLanes-1: 201 | VehicleArray[:,1:3] = big_state_matrix[1:-1,3:5] 202 | else: 203 | VehicleArray = big_state_matrix[1:NrLanes-1,HV_lane-1:HV_lane+2] 204 | 205 | elif HV_action == 'brk': 206 | # new_vehicle_Nr = rd.sample(range(NrLanes+1), 1) 207 | new_vehicle_Nr = rd.sample(range(NrLanes), 1) # up to NrLanes-1 EVs 208 | new_vehicle_pos = rd.sample(range(NrLanes), new_vehicle_Nr[0]) 209 | 210 | big_state_matrix[2,indices[0][1]] = 0 211 | big_state_matrix[3,indices[0][1]] = 2 212 | 213 | aux = np.array([[0,0,0,0,0]]) 214 | if len(EV_name_list)==0: 215 | max_EV_order = 0 216 | else: 217 | max_EV_order = max(EV_name_list) 218 | 219 | EV_name_list = [item for item in EV_name_list if item not in EV_name_list_first] 220 | EV_pos_list = np.delete(EV_pos_list, (EV_pos_row_first), axis=0) 221 | 222 | if new_vehicle_Nr[0] == 0: 223 | big_state_matrix = np.concatenate((big_state_matrix[1:5,:],aux)) 224 | EV_pos_list = EV_pos_list + [-1,0] 225 | else: 226 | EV_pos_list = EV_pos_list + [-1,0] 227 | # add_EV_pos_list = np.asarray([]) 228 | add_EV_pos_list = np.array([], dtype=np.int64).reshape(0,2) 229 | k = 0 230 | new_EV_order = [] 231 | for i in new_vehicle_pos: 232 | k = k+1 233 | aux[0][i] = 1 234 | EV_name_list.append(max_EV_order+k) # append new EV orders to the end 235 | EV_pos_list = np.concatenate((EV_pos_list,np.array([[4,i]]))) 236 | add_EV_pos_list = np.concatenate((add_EV_pos_list,np.array([[4,i]]))) # added EVs' positions 237 | new_EV_order.append(max_EV_order+k) 238 | # new_EV_order = list(np.asarray(new_vehicle_pos) + max_EV_order) 239 | big_state_matrix = np.concatenate((big_state_matrix[1:5,:],aux)) # assume only to show 5 rows of vehicles 240 | 241 | VehicleArray = np.zeros((3,3)) 242 | if HV_lane == 0: 243 | VehicleArray[:,0:2] = big_state_matrix[1:-1,0:2] 244 | elif HV_lane==NrLanes-1: 245 | VehicleArray[:,1:3] = big_state_matrix[1:-1,3:5] 246 | else: 247 | VehicleArray = big_state_matrix[1:NrLanes-1,HV_lane-1:HV_lane+2] 248 | 249 | state_aux = matrix2state(VehicleArray) 250 | return VehicleArray,state_aux,big_state_matrix,EV_name_list,new_EV_order,EV_pos_list,add_EV_pos_list 251 | 252 | #============================================================================== 253 | #============================================================================== 254 | # add new vehicles randomly to vacant grids before motions of all EVs 255 | 256 | def Add_EVs_front_back(big_state_matrix,HV_lane,NrLanes,EV_name_list_aux,EV_pos_list_aux): # to generate new state s_t+1 257 | 258 | new_EV_front_order = [-1] # indicate no new EV by default 259 | add_EV_pos_front_list = np.array([-1,-1]) 260 | new_EV_back_order = [-1] # indicate no new EV by default 261 | add_EV_pos_back_list = np.array([-1,-1]) 262 | EV_name_list = EV_name_list_aux +[] 263 | EV_pos_list = EV_pos_list_aux + 0 264 | 265 | big_state_matrix2 = big_state_matrix+0 # +0 to decouple VehicleArray_aux2 from VehicleArray_aux 266 | 267 | indices_front = np.asarray(np.where(big_state_matrix[0,:] == 0 )).T # vacant positions 268 | indices_back = np.asarray(np.where(big_state_matrix[-1,:] == 0 )).T # vacant positions 269 | 270 | if len(indices_front)>=1: 271 | Nr_new_vehicle_front = rd.sample(range(len(indices_front)), 1) # avoid full of EVs, at leat 1 vacant 272 | else: 273 | Nr_new_vehicle_front = rd.sample(range(len(indices_front)+1), 1) 274 | if len(indices_back)>=1: 275 | Nr_new_vehicle_back = rd.sample(range(len(indices_back)), 1) # avoid full of EVs, at leat 1 vacant 276 | else: 277 | Nr_new_vehicle_back = rd.sample(range(len(indices_back)+1), 1) 278 | 279 | if len(EV_name_list) == 0: 280 | max_EV_front_order = 0 281 | else: 282 | max_EV_front_order = max(EV_name_list) 283 | 284 | if Nr_new_vehicle_front[0] == 0: 285 | big_state_matrix = big_state_matrix2 286 | else: 287 | new_vehicle_pos_front = rd.sample(range(len(indices_front)), Nr_new_vehicle_front[0]) # take several number from 0~len(indices_front) 288 | add_EV_pos_front_list = np.array([], dtype=np.int64).reshape(0,2) #np.asarray([]) 289 | k = 0 290 | new_EV_front_order = [] 291 | for i in new_vehicle_pos_front: 292 | k = k+1 293 | big_state_matrix2[0,indices_front[i]] = 1 294 | # big_state_matrix3[indices_front[i][0],indices_front[i][1]] = -1 295 | add_EV_pos_front_list = np.concatenate((np.array([[0,indices_front[i]]]),add_EV_pos_front_list)) 296 | EV_name_list = [max_EV_front_order+k] + EV_name_list 297 | EV_pos_list = np.concatenate((np.array([[0,indices_front[i]]]),EV_pos_list)) 298 | new_EV_front_order = [max_EV_front_order+k]+new_EV_front_order 299 | # new_EV_front_order = list(np.asarray(new_vehicle_pos_front) + max_EV_front_order+k) 300 | big_state_matrix = big_state_matrix2 301 | 302 | if len(EV_name_list) == 0: 303 | max_EV_back_order = 0 304 | else: 305 | max_EV_back_order = max(EV_name_list) # front new EV included already 306 | if Nr_new_vehicle_back[0] == 0: 307 | big_state_matrix = big_state_matrix2 308 | else: 309 | new_vehicle_pos_back = rd.sample(range(len(indices_back)), Nr_new_vehicle_back[0]) 310 | add_EV_pos_back_list = np.array([], dtype=np.int64).reshape(0,2) #np.asarray([]) 311 | k = 0 312 | new_EV_back_order = [] 313 | for i in new_vehicle_pos_back: 314 | k = k+1 315 | big_state_matrix2[4,indices_back[i]] = 1 316 | add_EV_pos_back_list = np.concatenate((add_EV_pos_back_list,np.array([[4,indices_back[i]]]))) 317 | EV_name_list.append(max_EV_back_order+k) 318 | EV_pos_list = np.concatenate((EV_pos_list,np.array([[4,indices_back[i]]]))) 319 | new_EV_back_order.append(max_EV_back_order+k) 320 | big_state_matrix = big_state_matrix2 321 | 322 | return big_state_matrix,new_EV_front_order,new_EV_back_order,EV_name_list,EV_pos_list,add_EV_pos_front_list,add_EV_pos_back_list 323 | 324 | #============================================================================== 325 | #============================================================================== 326 | # safe actions of HV 327 | def Safe_actions_HV(state_aux): # HV_lane=0 <=> state_aux[0] = 1, HV_lane=right lane <=> state_aux[0] = 3 328 | 329 | VehicleArray_aux = state2matrix(state_aux) 330 | if state_aux[0] == 1: # fill 1 to occupy left boundary 331 | aux0 = np.concatenate(([1],VehicleArray_aux[0,:],[0])) 332 | aux1 = np.concatenate(([1],VehicleArray_aux[1,:],[0])) 333 | aux2 = np.concatenate(([1],VehicleArray_aux[2,:],[0])) 334 | VehicleArray_aux2 = np.array([[1,0,0,0,0],aux0,aux1,aux2,[1,0,0,0,0]]) #extended to 5x5 with zeros & ones, left boundary considered 335 | pos_HV = np.array([1,0]) 336 | elif state_aux[0] == 3: 337 | aux0 = np.concatenate(([0],VehicleArray_aux[0,:],[1])) 338 | aux1 = np.concatenate(([0],VehicleArray_aux[1,:],[1])) 339 | aux2 = np.concatenate(([0],VehicleArray_aux[2,:],[1])) 340 | VehicleArray_aux2 = np.array([[0,0,0,0,1],aux0,aux1,aux2,[0,0,0,0,1]]) #extended to 5x5 with zeros & ones, right boundary considered 341 | pos_HV = np.array([1,2]) 342 | elif state_aux[0] == 2: 343 | aux0 = np.concatenate(([0],VehicleArray_aux[0,:],[0])) 344 | aux1 = np.concatenate(([0],VehicleArray_aux[1,:],[0])) 345 | aux2 = np.concatenate(([0],VehicleArray_aux[2,:],[0])) 346 | VehicleArray_aux2 = np.array([[0,0,0,0,0],aux0,aux1,aux2,[0,0,0,0,0]]) #no boundary cases 347 | pos_HV = np.array([1,1]) 348 | 349 | pos_aux = pos_HV+[1,1] # new position in 5x5 array (5x5 not necessary, just make it uniform with context) 350 | 351 | pos_aux_acc = pos_aux + [-1,0] 352 | if VehicleArray_aux2[pos_aux_acc[0],pos_aux_acc[1]] == 0: 353 | acc = 1 354 | else: 355 | acc = 0 356 | pos_aux_brk = pos_aux + [1,0] 357 | if VehicleArray_aux2[pos_aux_brk[0],pos_aux_brk[1]] == 0: 358 | brk = 1 359 | else: 360 | brk = 0 361 | pos_aux_left = pos_aux + [0,-1] 362 | if VehicleArray_aux2[pos_aux_left[0],pos_aux_left[1]] == 0: 363 | left = 1 364 | else: 365 | left = 0 366 | 367 | pos_aux_right = pos_aux + [0,1] 368 | if VehicleArray_aux2[pos_aux_right[0],pos_aux_right[1]] == 0: 369 | right = 1 370 | else: 371 | right = 0 372 | 373 | return acc, brk, left, right 374 | 375 | #============================================================================== 376 | def Q2Policy(Q_value, ActionSet): 377 | a,b,c = Q_value.shape 378 | policy_act = [] 379 | 380 | for j in range(a): 381 | column = [] 382 | for i in range(c): 383 | column.append([]) 384 | policy_act.append(column) 385 | 386 | policy_index_aux = np.zeros([a,c]) 387 | policy_index = policy_index_aux.astype(int) 388 | 389 | for i in range(a): 390 | for j in range(c): 391 | Q_cur_state = Q_value[i,:,j] 392 | max_Q_cur_state = max(Q_cur_state) 393 | best_action_indices = [p for p, q in enumerate(Q_cur_state) if q == max_Q_cur_state] # best action direved from maximizing Q values of current state 394 | 395 | aux = best_action_indices[0] 396 | policy_index[i,j] = aux # just use the first action as policy 397 | policy_act[i][j] = ActionSet[policy_index[i,j]] 398 | 399 | return policy_act, policy_index 400 | #============================================================================== 401 | 402 | class NeuralNetwork: 403 | 404 | def __init__(self, layers, S, ActionSet, activation='tanh'): # make S 2-D, (3*320,10) 405 | if activation == 'sigmoid': 406 | self.activation = sigmoid 407 | self.activation_deriv = sigmoid_deriv 408 | elif activation == 'tanh': 409 | self.activation = tanh 410 | self.activation_deriv = tanh_deriv 411 | 412 | # Set weights 413 | self.weights = [] 414 | 415 | self.layers = layers 416 | layers_aux = np.asanyarray(layers)+1 417 | weight_num = sum(layers_aux[0:-1]*layers[1:len(layers)]) 418 | 419 | # layers = [2,2,1] 420 | # range of weight values (-1,1) 421 | # input and hidden layers - random((2+1, 2)) : 3 x 2, with bias in input & hidden layers 422 | for i in range(1, len(layers) - 1): 423 | r = 2*np.random.random((layers[i-1] + 1, layers[i])) -1 424 | self.weights.append(r) 425 | # output layer - random((2+1, 1)) : 3 x 1 426 | r = 2*np.random.random( (layers[i] + 1, layers[i+1])) - 1 427 | self.weights.append(r) # record all weight matrix: input_row * weight_matrix = row_output 428 | 429 | self.output_deriv = self.weights + [] # check if it is array or list 430 | self.output_deriv_matrix = np.zeros((S.shape[0]*len(ActionSet),weight_num)) 431 | 432 | 433 | def TrainNNBackProp(self, X, Y, learning_rate=0.2, epochs=100000): #epochs=5000000 434 | X = np.atleast_2d(X) 435 | temp = np.ones([X.shape[0], X.shape[1]+1]) 436 | temp[:, 0:-1] = X # adding the bias unit to the input layer. [index is up to -2] 437 | X = temp 438 | Y = np.array(Y) 439 | 440 | for k in range(epochs): 441 | i = np.random.randint(X.shape[0]) # i is random int upto lines of X 442 | a = [X[i]] # pick a row of input, random, with bias (output of input layer) 443 | 444 | for l in range(len(self.weights)): 445 | hidden_inputs = np.ones([self.weights[l].shape[1] + 1]) 446 | hidden_inputs[0:-1] = self.activation(np.dot(a[l], self.weights[l])) 447 | a.append(hidden_inputs) # append a, with bias at the end, net_outputs of [input,hidden, output layers] 448 | error = Y[i] - a[-1][:-1] # a[-1][:-1]: output of the output layer, ignore bias 449 | deltas = [error * self.activation_deriv(a[-1][:-1])] 450 | l = len(a) - 2 451 | 452 | # The last layer before the output is handled separately because of 453 | # the lack of bias node in output 454 | deltas.append(deltas[-1].dot(self.weights[l].T)*self.activation_deriv(a[l])) 455 | 456 | for l in range(len(a)-3, 0, -1): # we need to begin at the second to last layer 457 | deltas.append(deltas[-1][:-1].dot(self.weights[l].T)*self.activation_deriv(a[l])) 458 | 459 | deltas.reverse() 460 | for i in range(len(self.weights)-1): 461 | layer = np.atleast_2d(a[i]) 462 | delta = np.atleast_2d(deltas[i]) 463 | self.weights[i] += learning_rate * layer.T.dot(delta[:,:-1]) # layer.T.dot(delta[:,:-1]) is matrix 464 | 465 | # Handle last layer separately because it doesn't have a bias unit 466 | i+=1 467 | layer = np.atleast_2d(a[i]) 468 | delta = np.atleast_2d(deltas[i]) 469 | self.weights[i] += learning_rate * layer.T.dot(delta) 470 | 471 | def UpdWeiMaxEnt(self, S, ActionSet, mu_D, E_mu, learning_rate=0.2, L1_reg = -0.0, L2_reg = -0.0 ): #S is state table, 2D; Reward_SA: state_index*action*curvature 472 | 473 | #============================================================================== 474 | # X = np.atleast_2d(X) 475 | # X = np.zeros((S.shape[0]*S.shape[2],S.shape[1])) 476 | # X[0:S.shape[0],:] = S[:,:,0] 477 | # X[S.shape[0]:S.shape[0]*2,:] = S[:,:,1] 478 | # X[S.shape[0]*2:S.shape[0]*3,:] = S[:,:,2] 479 | 480 | temp = np.ones([S.shape[0], S.shape[1]+1]) 481 | temp[:, 0:-1] = S # adding the bias unit to the input layer. [index is up to -2] 482 | X = temp 483 | 484 | # Reward_SA is useless here, since we don't use error function Error(output) 485 | # Y = np.zeros((Reward_SA.shape[0]*Reward_SA.shape[2],Reward_SA.shape[1])) 486 | # Y[0:Reward_SA.shape[0],:] = Reward_SA[:,:,0] 487 | # Y[Reward_SA.shape[0]:Reward_SA.shape[0]*2,:] = Reward_SA[:,:,1] 488 | # Y[Reward_SA.shape[0]*2:Reward_SA.shape[0]*3,:] = Reward_SA[:,:,2] 489 | 490 | 491 | for j in range(len(ActionSet)): # actions 492 | for k in range(X.shape[0]): 493 | 494 | output_deriv_aux = [] 495 | 496 | a = [X[k]] # pick a row of input, random, with bias (output of input layer). CHECK IF A ROW 497 | 498 | for l in range(len(self.weights)): 499 | hidden_inputs = np.ones([self.weights[l].shape[1] + 1]) 500 | hidden_inputs[0:-1] = self.activation(np.dot(a[l], self.weights[l])) 501 | a.append(hidden_inputs) # append a, with bias at the end, net_outputs of [input,hidden, output layers] 502 | # error = y[i] - a[-1][:-1] # a[-1][:-1]: output of the output layer, ignore bias 503 | FakeOutSwitch = np.zeros(len(ActionSet)) 504 | FakeOutSwitch[j] = 1 505 | deltas = [FakeOutSwitch * self.activation_deriv(a[-1][:-1])] 506 | l = len(a) - 2 # index of the last 2nd layer 507 | 508 | # The last layer before the output is handled separately because of 509 | # the lack of bias node in output 510 | deltas.append(deltas[-1].dot(self.weights[l].T)*self.activation_deriv(a[l])) 511 | 512 | for l in range(len(a) -3, 0, -1): # we need to begin at the second to last layer 513 | deltas.append(deltas[-1][:-1].dot(self.weights[l].T)*self.activation_deriv(a[l])) 514 | 515 | deltas.reverse() 516 | 517 | for i in range(len(self.weights)-1): 518 | layer = np.atleast_2d(a[i]) # take i th layer output row vector 519 | delta = np.atleast_2d(deltas[i]) 520 | self.output_deriv[i] = layer.T.dot(delta[:,:-1]) # this is correct 521 | output_deriv_aux=np.concatenate([output_deriv_aux,np.concatenate(nn.output_deriv[i].T)]) 522 | 523 | # Handle last layer separately because it doesn't have a bias unit 524 | i+=1 525 | layer = np.atleast_2d(a[i]) 526 | delta = np.atleast_2d(deltas[i]) 527 | self.output_deriv[i] = layer.T.dot(delta) 528 | output_deriv_aux=np.concatenate([output_deriv_aux,np.concatenate(nn.output_deriv[i].T)]) 529 | 530 | self.output_deriv_matrix[j*X.shape[0]+k,:] = output_deriv_aux + 0 531 | 532 | #============================== learn the weights ============================= 533 | 534 | # rescale = np.exp(learning_rate*(mu_D-E_mu).dot(self.output_deriv_matrix)) # watch dimensions 535 | # index = 0 536 | # for i in range(len(self.layers)-1): 537 | # rows = self.weights[i].shape[0] 538 | # columns = self.weights[i].shape[1] 539 | # Aux = rescale[0,index:index+rows*columns] + 0 540 | # Aux2 = self.weights[i]+0 541 | # self.weights[i] *= np.reshape(Aux,(columns,rows)).T 542 | # self.weights[i] += L2_reg*Aux2 543 | # index += rows*columns 544 | 545 | rescale = learning_rate*(mu_D-E_mu).dot(self.output_deriv_matrix) # watch dimensions 546 | index = 0 547 | for i in range(len(self.layers)-1): 548 | rows = self.weights[i].shape[0] 549 | columns = self.weights[i].shape[1] 550 | Aux = rescale[0,index:index+rows*columns] + 0 551 | self.weights[i] += ( np.reshape(Aux,(columns,rows)).T+L1_reg*np.sign(self.weights[i])+L2_reg*self.weights[i] ) 552 | index += rows*columns 553 | 554 | 555 | def predict(self, x): 556 | a = np.array(x) 557 | for j in range(0, len(self.weights)): 558 | temp = np.ones(a.shape[0]+1) 559 | temp[0:-1] = a 560 | a = self.activation(np.dot(temp, self.weights[j])) 561 | return a 562 | 563 | #=========================== begin define EV ================================ 564 | class EV(object): 565 | def __init__(self, Available_actions_fn, big_state_matrix, ActionSet, Pr_actions, big_pos, HV_lane, NrLanes): 566 | self.Pr_stay = Pr_actions[-1] 567 | self.Pr_acc = Pr_actions[-2] 568 | self.Pr_left = Pr_actions[-3] 569 | self.Pr_right = Pr_actions[-4] 570 | self.Pr_brake = Pr_actions[-5] 571 | self.Available_actions_fn=Available_actions_fn 572 | self.big_pos = big_pos 573 | # self.color_index = color_index 574 | # self.Color = ColorSet[self.color_index] 575 | self.Action = 'stay' 576 | self.lane = big_pos[1] 577 | 578 | def TakeAction(self,big_state_matrix,HV_lane,NrLanes,ActionSet,Pr_actions): # use new state when take action everytime, update lane and pos 579 | big_pos = self.big_pos 580 | self.Pr_stay = Pr_actions[-1] 581 | self.Pr_acc = Pr_actions[-2] 582 | self.Pr_left = Pr_actions[-3] 583 | self.Pr_right = Pr_actions[-4] 584 | self.Pr_brake = Pr_actions[-5] 585 | 586 | acc, brk, left, right = self.Available_actions_fn(big_state_matrix,big_pos,HV_lane,NrLanes) 587 | Pr_sum = sum([brk, right, left, acc, 1]*Pr_actions) # total = 1 if everything available 588 | 589 | self.Pr_stay = self.Pr_stay/Pr_sum 590 | self.Pr_acc = self.Pr_acc/Pr_sum*acc # ->0 if acc=0 591 | self.Pr_left = self.Pr_left/Pr_sum*left 592 | self.Pr_right = self.Pr_right/Pr_sum*right 593 | self.Pr_brake = self.Pr_brake/Pr_sum*brk 594 | 595 | act = np.random.random() # generate an action number randomly 596 | eps = 1e-8 597 | aux1 = self.Pr_stay+eps 598 | aux2 = aux1+self.Pr_acc+eps 599 | aux3 = aux2+self.Pr_brake+eps 600 | aux4 = aux3+self.Pr_left+eps 601 | aux5 = aux4+self.Pr_right+eps # aux5 == 1+5*eps 602 | bins = np.array([0, aux1, aux2, aux3, aux4, aux5]) 603 | index = np.digitize(act,bins) 604 | self.Action = ActionSet[index-1] 605 | 606 | if self.Action=='stay': 607 | self.big_pos = big_pos 608 | elif self.Action=='acc': 609 | self.big_pos = big_pos+[-1,0] 610 | elif self.Action=='brk': 611 | self.big_pos = big_pos+[1,0] 612 | elif self.Action=='left': 613 | self.big_pos = big_pos+[0,-1] 614 | self.lane = self.lane-1 615 | elif self.Action=='right': 616 | self.big_pos = big_pos+[0,1] 617 | self.lane = self.lane+1 618 | 619 | #============================= end define EV ================================= 620 | #=========================== begin define HV ================================ 621 | 622 | class HV_obj(object): 623 | def __init__(self, Safe_actions_fn, Q_value, Curvature_Class, state, HV_lane, ActionSet, Nr_action, epsilon=1e-3, alpha=0.5, gamma=0.9, NrLanes=5): # for demonstraion purpose, check available actions so that no collision happens 624 | # modify Available_actions_fn with all actions for other purpose 625 | self.Q_value = Q_value 626 | self.epsilon = epsilon 627 | self.alpha = alpha 628 | self.gamma = gamma 629 | self.HV_lane = HV_lane 630 | self.collision = 0 631 | self.Action = 'stay' 632 | self.Safe_actions_fn = Safe_actions_fn 633 | self.Curvature_Class = Curvature_Class 634 | 635 | if state[0] == 1: 636 | self.HV_pos = np.array([1,0]) 637 | self.HV_big_pos = np.array([2,0]) 638 | elif state[0] == 2: 639 | self.HV_pos = np.array([1,1]) 640 | self.HV_big_pos = np.array([2,HV_lane]) 641 | else: 642 | self.HV_pos = np.array([1,2]) 643 | self.HV_big_pos = np.array([2,NrLanes-1]) 644 | 645 | def TakeAction_HV(self,state,RewardVector,Nr_action,ActionSet): 646 | # state_row_index = 2**8*(state[0]-1)+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*state[1:9] ) 647 | if state[0] == 1: 648 | state_row_index = sum( np.array([2**4,2**3,0,2**2,0,2,1,0])*state[1:9] ) 649 | elif state[0] == 2: 650 | state_row_index = 2**5+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*state[1:9] ) 651 | elif state[0] == 3: 652 | state_row_index = 2**5+2**8+sum( np.array([0,2**4,2**3,0,2**2,0,2,1])*state[1:9] ) 653 | 654 | Q_cur_state = self.Q_value[int(round(state_row_index)),:,self.Curvature_Class] 655 | max_Q_cur_state = max(Q_cur_state) 656 | 657 | best_action_indices = [i for i, j in enumerate(Q_cur_state) if j == max_Q_cur_state] # best action direved from maximizing Q values of current state 658 | 659 | act = np.random.random() # generate an action number randomly, to use epsilon-greedy 660 | 661 | if len(best_action_indices) == Nr_action: 662 | bins = np.linspace(0,1,Nr_action+1) 663 | index = np.digitize(act,bins) 664 | self.Action = ActionSet[index-1] 665 | action_index = index-1 666 | else: 667 | other_action_indices = [item for item in [0,1,2,3,4] if item not in best_action_indices] 668 | bin1 = np.linspace(0,1-self.epsilon,len(best_action_indices)+1) 669 | bin2 = np.linspace(1-self.epsilon,1,len(other_action_indices)+1) 670 | bins = np.concatenate((bin1,bin2[1:len(bin2)])) 671 | index = np.digitize(act,bins) 672 | if index <= len(bin1)-1: 673 | action_index = best_action_indices[index-1] 674 | self.Action = ActionSet[action_index] 675 | else: 676 | action_index = other_action_indices[index-len(bin1)] 677 | self.Action = ActionSet[action_index] 678 | 679 | self.Q_index = [int(round(state_row_index)),action_index] # the position of Q to update 680 | 681 | # Reward,Feature = self.Reward_fn(state,self.HV_lane,self.Action,self.theta,self.Curvature_Class,NrLanes) 682 | 683 | acc, brk, left, right = self.Safe_actions_fn(state) 684 | Safe_actions_set = ['stay'] 685 | if acc == 1: 686 | Safe_actions_set.append('acc') 687 | if brk == 1: 688 | Safe_actions_set.append('brk') 689 | if left == 1: 690 | Safe_actions_set.append('left') 691 | if right == 1: 692 | Safe_actions_set.append('right') 693 | 694 | if self.Action in Safe_actions_set: 695 | self.collision = 0 696 | else: 697 | self.collision = 1 698 | 699 | index_aux = ActionSet.index(self.Action) 700 | self.ImmediateReward = RewardVector[index_aux] 701 | 702 | pos = self.HV_pos 703 | big_pos = self.HV_big_pos 704 | if self.Action=='stay': 705 | self.HV_pos = pos 706 | self.HV_big_pos = big_pos 707 | elif self.Action=='acc': 708 | self.HV_pos = pos+[-1,0] 709 | self.HV_big_pos = big_pos+[-1,0] 710 | elif self.Action=='brk': 711 | self.HV_pos = pos+[1,0] 712 | self.HV_big_pos = big_pos+[1,0] 713 | elif self.Action=='left': 714 | self.HV_pos = pos+[0,-1] 715 | self.HV_big_pos = big_pos+[0,-1] 716 | self.HV_lane = self.HV_lane-1 717 | elif self.Action=='right': 718 | self.HV_pos = pos+[0,1] 719 | self.HV_big_pos = big_pos+[0,1] 720 | self.HV_lane = self.HV_lane+1 721 | 722 | #=============================== end define HV ================================ 723 | 724 | if __name__ == '__main__': #global variable indicates the entry point of the program 725 | 726 | np.random.seed(0) # make it reproducible 727 | Nr_action = 5 728 | Pr_actions = np.random.rand(Nr_action) 729 | Pr_actions = Pr_actions/sum(Pr_actions) # normalize action probabilities 730 | Pr_actions.sort() # increasing order 731 | NrLanes = 5 # road of 5 lanes, 0~4 from left to right 732 | FixVehicleNr = False 733 | 734 | ActionSet = ['stay','acc','brk','left','right'] 735 | mini_state = np.loadtxt('IRL_temp_folder/save_MinState_table.txt') 736 | demo_states = np.load('IRL_temp_folder/save_demo_states.npy') # contain curvature 737 | demo_actions = np.load('IRL_temp_folder/save_demo_actions.npy') # action indices 738 | # print(demo_states.shape) 739 | 740 | # define whole state table 741 | S = np.zeros((mini_state.shape[0]*3,mini_state.shape[1]+1)) 742 | S[0:mini_state.shape[0],0:-1] = mini_state 743 | S[mini_state.shape[0]:mini_state.shape[0]*2,0:-1] = mini_state 744 | S[mini_state.shape[0]*2:mini_state.shape[0]*3,0:-1] = mini_state 745 | 746 | S[0:mini_state.shape[0],-1] = np.zeros((mini_state.shape[0])) # left 747 | S[mini_state.shape[0]:mini_state.shape[0]*2,-1] = np.ones((mini_state.shape[0])) # straight 748 | S[mini_state.shape[0]*2:mini_state.shape[0]*3,-1] = 2*np.ones((mini_state.shape[0])) #right 749 | 750 | # calculate mu_D from demos 751 | 752 | # mu_D_S = np.zeros((1,S.shape[0])) # define a row 753 | # mu_D_SA = np.zeros((1,S.shape[0]*len(ActionSet))) # define a row 754 | # for j in range(demo_states.shape[1]): # number of demo trajectories 755 | # for i in range(demo_states.shape[0]): # number of demo steps 756 | # if demo_states[i,j,0] == 1: 757 | # state_row_index = sum( np.array([2**4,2**3,0,2**2,0,2,1,0])*demo_states[i,j,1:9] )+mini_state.shape[0]*demo_states[i,j,9] 758 | # elif demo_states[i,j,0] == 2: 759 | # state_row_index = 2**5+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*demo_states[i,j,1:9] )+mini_state.shape[0]*demo_states[i,j,9] 760 | # elif demo_states[i,j,0] == 3: 761 | # state_row_index = 2**5+2**8+sum( np.array([0,2**4,2**3,0,2**2,0,2,1])*demo_states[i,j,1:9] )+mini_state.shape[0]*demo_states[i,j,9] 762 | # mu_D_S[0,int(round(state_row_index))] += 1 763 | # mu_D_SA[0,int(round(state_row_index))+int(round(demo_actions[i,j]))*S.shape[0]] += 1 764 | # mu_D_S /= demo_states.shape[1] 765 | # mu_D_SA /= demo_states.shape[1] 766 | 767 | #================================== Mac OS===================================== 768 | mu_D_S = np.load('IRL_temp_folder/save_mu_D_S.npy') 769 | mu_D_SA = np.load('IRL_temp_folder/save_mu_D_SA.npy') 770 | #================================= Windows ==================================== 771 | # mu_D_S = np.load('IRL_temp_folder\save_mu_D_S.npy') 772 | # mu_D_SA = np.load('IRL_temp_folder\save_mu_D_SA.npy') 773 | #============================================================================== 774 | 775 | # mu_D_SA_unify = mu_D_SA+0 776 | # for i in range(S.shape[0]): 777 | # if mu_D_S[0,i] == 0: 778 | # aux = 1 779 | # else: 780 | # aux = mu_D_S[0,i]+0 781 | # mu_D_SA_unify[0,i] = mu_D_SA[0,i]/aux 782 | # mu_D_SA_unify[0,i+S.shape[0]] = mu_D_SA[0,i+S.shape[0]]/aux 783 | # mu_D_SA_unify[0,i+S.shape[0]*2] = mu_D_SA[0,i+S.shape[0]*2]/aux 784 | # mu_D_SA_unify[0,i+S.shape[0]*3] = mu_D_SA[0,i+S.shape[0]*3]/aux 785 | # mu_D_SA_unify[0,i+S.shape[0]*4] = mu_D_SA[0,i+S.shape[0]*4]/aux 786 | 787 | #================================== Mac OS===================================== 788 | mu_D_SA_unify = np.load('IRL_temp_folder/mu_D_SA_unify.npy') 789 | #================================= Windows ==================================== 790 | # mu_D_SA_unify = np.load('IRL_temp_folder\mu_D_SA_unify.npy') 791 | #============================================================================== 792 | 793 | # Initialize NN reward function 794 | nn_layers = [10,20,20,20,5] 795 | nn = NeuralNetwork(nn_layers,S,ActionSet) 796 | 797 | # Try Supervised Learning to get weights 798 | 799 | # Demo_Y = np.zeros((S.shape[0],len(ActionSet))) 800 | # for i in range(S.shape[0]): 801 | # Demo_Y[i,0] = mu_D_SA_unify[0,i] 802 | # Demo_Y[i,1] = mu_D_SA_unify[0,i+S.shape[0]] 803 | # Demo_Y[i,2] = mu_D_SA_unify[0,i+S.shape[0]*2] 804 | # Demo_Y[i,3] = mu_D_SA_unify[0,i+S.shape[0]*3] 805 | # Demo_Y[i,4] = mu_D_SA_unify[0,i+S.shape[0]*4] 806 | 807 | #================================== Mac OS===================================== 808 | Demo_Y = np.load('IRL_temp_folder/Demo_Y.npy') 809 | #================================= Windows ==================================== 810 | # Demo_Y = np.load('IRL_temp_folder\Demo_Y.npy') 811 | #============================================================================== 812 | # initialize NN 813 | nn.TrainNNBackProp(S, Demo_Y, 0.1) 814 | Pred_Y = np.zeros((S.shape[0],len(ActionSet))) 815 | for i in range(S.shape[0]): 816 | Pred_Y[i,:] = nn.predict(S[i,:]) 817 | print('Train error is {:f}'.format(np.linalg.norm(Demo_Y-Pred_Y))) 818 | print('========= Epoch {:d} begin ========='.format(1)) 819 | 820 | #================================== Mac OS===================================== 821 | # for i in range(len(nn.weights)): 822 | # nn.weights[i] = np.load('IRL_temp_folder/save_reward_fcn{}.npy'.format(i)) 823 | #================================= Windows ==================================== 824 | # for i in range(len(nn.weights)): 825 | # nn.weights[i] = np.load('IRL_temp_folder\save_reward_fcn{}.npy'.format(i)) 826 | #============================================================================== 827 | 828 | 829 | #============================================================================== 830 | #============================== begin IRL here ================================ 831 | #============================================================================== 832 | 833 | learn_iters = 200 #demo_states.shape[0] 834 | Curve_Last_Steps = 5 835 | 836 | # T_count = np.zeros((S.shape[0],S.shape[0],len(ActionSet))) # define state transition matrix, learned on-line, keeping visit number to each state-action pair 837 | #================================== Mac OS===================================== 838 | T_count = np.load('IRL_temp_folder/save_T_count.npy') 839 | #================================= Windows ==================================== 840 | # T_count = np.load('IRL_temp_folder\save_T_count.npy') 841 | #============================================================================== 842 | 843 | # Initialize Q-learning 844 | Initial_state = np.array([2,1,0,1,0,1,0,1,0 ]) 845 | Initial_state_matrix = state2matrix(Initial_state) 846 | aux0 = np.concatenate(([0],Initial_state_matrix[0,:],[0])) 847 | aux1 = np.concatenate(([0],Initial_state_matrix[1,:],[0])) 848 | aux2 = np.concatenate(([0],Initial_state_matrix[2,:],[0])) 849 | Initial_big_state_matrix = np.array([[1,0,0,0,1],aux0,aux1,aux2,[0,1,0,1,0]]) 850 | 851 | Curvature_Class = 0 852 | 853 | Initial_HV_lane = NrLanes-3 # HV_lane range from 0-4 854 | Nr_action = 5 # total number of actions 855 | 856 | # initialize E_mu_S: state visit count, E_mu_SA: state-action visit count 857 | Initial_E_mu_S = np.zeros((1,S.shape[0])) # define a row 858 | if Initial_state[0] == 1: 859 | Initial_state_row_index = sum( np.array([2**4,2**3,0,2**2,0,2,1,0])*Initial_state[1:9] )+mini_state.shape[0]*Curvature_Class 860 | elif Initial_state[0] == 2: 861 | Initial_state_row_index = 2**5+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*Initial_state[1:9] )+mini_state.shape[0]*Curvature_Class 862 | elif Initial_state[0] == 3: 863 | Initial_state_row_index = 2**5+2**8+sum( np.array([0,2**4,2**3,0,2**2,0,2,1])*Initial_state[1:9] )+mini_state.shape[0]*Curvature_Class 864 | Initial_E_mu_S[0,Initial_state_row_index] = 1 865 | 866 | for n in range(learn_iters): 867 | 868 | #============================= begin Q-learning =============================== 869 | # Initialize Q-learning 870 | 871 | epsilon = 8e-2 872 | alpha = 0.75 873 | gamma = 0.5 874 | 875 | Q_value = np.zeros((2**8+2*2**5,Nr_action,3)) # maintain a full Q table 876 | 877 | # implement Q-learning 878 | converge = False 879 | Episode = 0 880 | criterion_max = 1e-2 881 | 882 | while not converge: 883 | 884 | Episode_over = False 885 | step = 0 886 | 887 | Find_EV_indices = np.asarray(np.where(Initial_big_state_matrix == 1 )).T 888 | Initial_EV = len(Find_EV_indices) # total number of EVs 889 | EV_name_list = list(range(Initial_EV)) # name list of active EVs 890 | EV_pos_list = Find_EV_indices 891 | 892 | HV = HV_obj(Safe_actions_HV, Q_value, Curvature_Class, Initial_state, Initial_HV_lane, ActionSet, Nr_action, epsilon, alpha, gamma, NrLanes) 893 | EV_set={} 894 | for x in range(0,Initial_EV): 895 | EV_pos = np.array(Find_EV_indices[x]) 896 | EV_set["EV{}".format(x)]=EV(Available_actions_EV, Initial_big_state_matrix, ActionSet, Pr_actions, EV_pos, Initial_HV_lane, NrLanes) 897 | 898 | state = Initial_state + 0 899 | big_state_matrix = Initial_big_state_matrix + 0 900 | # HV.HV_lane = Initial_HV_lane + 0 901 | # HV.alpha = alpha + 0 902 | Q_cur = HV.Q_value + 0 903 | 904 | while not Episode_over: 905 | 906 | step=step+1 907 | Curvature_Class_aux = (step//Curve_Last_Steps)%3 908 | HV.Curvature_Class = Curvature_Class_aux+0 909 | 910 | # 911 | state_combine = np.zeros(10) 912 | state_combine[0:-1] = state+0 913 | state_combine[-1] = HV.Curvature_Class 914 | 915 | RewardVector = nn.predict(state_combine) 916 | HV_pos_before = HV.HV_big_pos + 0 917 | HV.TakeAction_HV(state,RewardVector,Nr_action,ActionSet) 918 | HV_pos_after = HV.HV_big_pos + 0 919 | 920 | if HV.collision == 1: 921 | # HV.Q_value[HV.Q_index[0],HV.Q_index[1],] = HV.ImmediateReward+0 922 | HV.Q_value[HV.Q_index[0],HV.Q_index[1],] = -10 923 | Episode_over = True 924 | else: 925 | # no collison, get T matrix indices for updating 926 | if state[0] == 1: 927 | state_row_index_pre = sum( np.array([2**4,2**3,0,2**2,0,2,1,0])*state[1:9] )+mini_state.shape[0]*HV.Curvature_Class 928 | elif state[0] == 2: 929 | state_row_index_pre = 2**5+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*state[1:9] )+mini_state.shape[0]*HV.Curvature_Class 930 | elif state[0] == 3: 931 | state_row_index_pre = 2**5+2**8+sum( np.array([0,2**4,2**3,0,2**2,0,2,1])*state[1:9] )+mini_state.shape[0]*HV.Curvature_Class 932 | trans_act_index = ActionSet.index(HV.Action) 933 | 934 | EV_name_list1 = EV_name_list + [] 935 | EV_pos_list1 = EV_pos_list + 0 936 | VehicleArray,state_aux,big_state_matrix,EV_name_list,new_EV_order,EV_pos_list,add_EV_pos_list = Add_Vehicle_HV_move(state,big_state_matrix,HV.HV_lane,HV.Action,NrLanes,EV_name_list,EV_pos_list) 937 | EV_name_list2 = EV_name_list + [] 938 | EV_pos_list2 = EV_pos_list + 0 939 | 940 | #============== begin update Q-values using expected state ==================== 941 | # expected next state after HV move, only used for calculate Q_max for next state 942 | expected_array = VehicleArray+0 943 | if HV.Action == 'acc': 944 | expected_array[0,:] = np.array([0,0,0]) 945 | elif HV.Action == 'brk': 946 | expected_array[-1,:] = np.array([0,0,0]) 947 | elif HV.Action == 'left': 948 | if HV.HV_lane != 0: 949 | expected_array[:,0] = np.array([0,0,0]).transpose() 950 | else: 951 | expected_array[:,-1] = np.array([0,0,0]).transpose() 952 | elif HV.Action == 'right': 953 | if HV.HV_lane != NrLanes-1: 954 | expected_array[:,-1] = np.array([0,0,0]).transpose() 955 | else: 956 | expected_array[:,0] = np.array([0,0,0]).transpose() 957 | expected_state = matrix2state(expected_array) 958 | 959 | # update Q value 960 | if expected_state[0] == 1: 961 | state_row_index = sum( np.array([2**4,2**3,0,2**2,0,2,1,0])*expected_state[1:9] ) 962 | elif expected_state[0] == 2: 963 | state_row_index = 2**5+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*expected_state[1:9] ) 964 | elif expected_state[0] == 3: 965 | state_row_index = 2**5+2**8+sum( np.array([0,2**4,2**3,0,2**2,0,2,1])*expected_state[1:9] ) 966 | 967 | Q_nxt_state = HV.Q_value[int(round(state_row_index)),:,HV.Curvature_Class] 968 | max_Q_nxt_state = max(Q_nxt_state) 969 | # HV.alpha = 1/( 1+step )**0.75 970 | aux = (1-HV.alpha)*HV.Q_value[HV.Q_index[0],HV.Q_index[1],HV.Curvature_Class]+HV.alpha*(HV.ImmediateReward+HV.gamma*max_Q_nxt_state) 971 | HV.Q_value[HV.Q_index[0],HV.Q_index[1],HV.Curvature_Class] = aux 972 | 973 | #================ end update Q-values using expected state ==================== 974 | 975 | # Initialize new EVs 976 | if new_EV_order != [-1]: # new EVs found due to acc or brk 977 | k = 0 978 | for x in new_EV_order: 979 | EV_pos = add_EV_pos_list[k,:] 980 | EV_set["EV{}".format(x)]=EV(Available_actions_EV, big_state_matrix, ActionSet, Pr_actions, EV_pos, HV.HV_lane, NrLanes) 981 | k = k+1 982 | 983 | # reset HV position 984 | if HV.Action == 'brk' or HV.Action == 'stay' or HV.Action == 'acc': 985 | HV.HV_big_pos = HV_pos_before + 0 # reset HV pos 986 | 987 | # add new EVs from front and back to vacants 988 | big_state_matrix,new_EV_front_order,new_EV_back_order,EV_name_list,EV_pos_list,add_EV_pos_front_list,add_EV_pos_back_list = Add_EVs_front_back(big_state_matrix,HV.HV_lane,NrLanes,EV_name_list2,EV_pos_list2) 989 | EV_name_list3 = EV_name_list + [] 990 | EV_pos_list3 = EV_pos_list + 0 991 | 992 | # Initialize new EVs 993 | if new_EV_front_order != [-1]: # new added EVs found to front 994 | k = 0 995 | for x in new_EV_front_order: 996 | EV_pos = add_EV_pos_front_list[k,:] 997 | EV_set["EV{}".format(x)]=EV(Available_actions_EV, big_state_matrix, ActionSet, Pr_actions, EV_pos, HV.HV_lane, NrLanes) 998 | k = k+1 999 | if new_EV_back_order != [-1]: # new added EVs found to back 1000 | k = 0 1001 | for x in new_EV_back_order: 1002 | EV_pos = add_EV_pos_back_list[k,:] 1003 | EV_set["EV{}".format(x)]=EV(Available_actions_EV, big_state_matrix, ActionSet, Pr_actions, EV_pos, HV.HV_lane, NrLanes) 1004 | k = k+1 1005 | 1006 | # all original EVs take an action, except for newly found EVs, results stored in big array 1007 | VehicleArray_aux = np.zeros((7,7)) 1008 | VehicleArray_aux[1:6,1:6] = big_state_matrix + 0 1009 | 1010 | VehicleArray_aux2 = VehicleArray_aux + 0 1011 | big_state_matrix_aux = big_state_matrix + 0 # keep original posiition of EV after moving, avoiding cross in animation 1012 | 1013 | k = 0 1014 | EV_pos_list_aux = np.zeros((len(EV_name_list2),2)).astype(int) 1015 | for x in EV_name_list2: 1016 | EV_pos = np.array(EV_pos_list2[k,:]) 1017 | EV_pos_list_aux[k,:] = EV_set["EV{}".format(x)].big_pos + 0 # check if this works properly 1018 | EV_set["EV{}".format(x)].big_pos = EV_pos+0 # check if this works, update positon of identified EVs since HV moves 1019 | k = k+1 1020 | 1021 | k = 0 1022 | for x in EV_name_list2: 1023 | EV_pos = np.array(EV_pos_list2[k,:]) 1024 | EV_pos_aux = EV_pos+[1,1] 1025 | VehicleArray_aux[EV_pos_aux[0],EV_pos_aux[1]] = 0 # remove original EV that moves in the big array 1026 | VehicleArray_aux2[EV_pos_aux[0],EV_pos_aux[1]] = -1 # -1 inidicate there was an EV 1027 | 1028 | EV_set["EV{}".format(x)].TakeAction(big_state_matrix_aux,HV.HV_lane,NrLanes,ActionSet,Pr_actions) # EV takes action 1029 | EV_pos_aux = EV_set["EV{}".format(x)].big_pos+[1,1] # new pos in big array 1030 | 1031 | VehicleArray_aux[EV_pos_aux[0],EV_pos_aux[1]] = 1 # set new pos in big array in big array = 1 1032 | big_state_matrix = VehicleArray_aux[1:6,1:6] + 0 1033 | 1034 | VehicleArray_aux2[EV_pos_aux[0],EV_pos_aux[1]] = 1 1035 | big_state_matrix_aux = VehicleArray_aux2[1:6,1:6] + 0 # remember that big_state_matrix_aux is not true big_state_matrix 1036 | 1037 | k = k+1 1038 | ######################################### Version 2 END ########################################## 1039 | 1040 | # update new state after all EVs moving 1041 | VehicleArray = np.zeros((3,3)) 1042 | if HV.HV_lane == 0: 1043 | VehicleArray[:,0:2] = big_state_matrix[1:-1,0:2] 1044 | elif HV.HV_lane==NrLanes-1: 1045 | VehicleArray[:,1:3] = big_state_matrix[1:-1,3:5] 1046 | else: 1047 | VehicleArray = big_state_matrix[1:NrLanes-1,HV.HV_lane-1:HV.HV_lane+2] 1048 | 1049 | state = matrix2state(VehicleArray) # new state after each EV moves 1050 | 1051 | # ready to update T after renewing the state 1052 | if state[0] == 1: 1053 | state_row_index_post = sum( np.array([2**4,2**3,0,2**2,0,2,1,0])*state[1:9] )+mini_state.shape[0]*HV.Curvature_Class 1054 | elif state[0] == 2: 1055 | state_row_index_post = 2**5+sum( np.array([2**7,2**6,2**5,2**4,2**3,2**2,2,1])*state[1:9] )+mini_state.shape[0]*HV.Curvature_Class 1056 | elif state[0] == 3: 1057 | state_row_index_post = 2**5+2**8+sum( np.array([0,2**4,2**3,0,2**2,0,2,1])*state[1:9] )+mini_state.shape[0]*HV.Curvature_Class 1058 | 1059 | T_count[int(round(state_row_index_pre)),int(round(state_row_index_post)),trans_act_index] += 1 1060 | 1061 | # remove list elements that disappear from the front and back by acc or brk 1062 | k = 0 1063 | list_delete = [] 1064 | pos_row_delete = [] 1065 | pos_replace = np.array([], dtype=np.int64).reshape(0,2) 1066 | for x in EV_name_list2: 1067 | if EV_set["EV{}".format(x)].big_pos[0]<0 or EV_set["EV{}".format(x)].big_pos[0]>4: 1068 | list_delete = list_delete + [x] 1069 | pos_row_delete = pos_row_delete + [k] 1070 | elif EV_set["EV{}".format(x)].big_pos[0]>=0 and EV_set["EV{}".format(x)].big_pos[0]<=4: # replace elements in list3 1071 | pos_replace = np.concatenate((pos_replace,np.array([[EV_set["EV{}".format(x)].big_pos[0],EV_set["EV{}".format(x)].big_pos[1]]]))) 1072 | 1073 | EV_name_list = [item for item in EV_name_list3 if item not in list_delete] 1074 | 1075 | if new_EV_front_order == [-1]: 1076 | EV_pos_list = pos_replace 1077 | else: 1078 | EV_pos_list = np.concatenate((add_EV_pos_front_list,pos_replace)) 1079 | if new_EV_back_order != [-1]: 1080 | EV_pos_list = np.concatenate((EV_pos_list,add_EV_pos_back_list)) 1081 | 1082 | Episode = Episode+1 1083 | criterion = np.linalg.norm(Q_cur-HV.Q_value) 1084 | if Episode//500 > (Episode-1)//500: 1085 | print('Episode:',Episode,'step:',step,'criterion:{:f}'.format(criterion)) 1086 | if criterion 5e3: 1087 | converge = True 1088 | 1089 | #================================== Mac OS===================================== 1090 | np.save('IRL_temp_folder/save_T_count.npy',T_count) 1091 | #================================= Windows ==================================== 1092 | # np.save('IRL_temp_folder\save_T_count.npy',T_count) 1093 | #============================================================================== 1094 | 1095 | # obtain policy 1096 | policy_act, policy_index = Q2Policy(HV.Q_value, ActionSet) 1097 | 1098 | #======================= save train result every epoch ======================== 1099 | Q_value = HV.Q_value + 0 1100 | #============================= Q learning done ================================ 1101 | #============================================================================== 1102 | #============================================================================== 1103 | 1104 | # policy_index = np.loadtxt('IRL_temp_folder/save_optimal_policy.txt') # use optimal polisy to verify T 1105 | 1106 | #============================================================================== 1107 | #=================== policy propagation & calculate E_mu ====================== 1108 | #============================================================================== 1109 | 1110 | policy_index_vector = np.reshape(policy_index.T,(S.shape[0])) # this is a row 1111 | # E_mu_SA_matrix = np.zeros((S.shape[0]*len(ActionSet),demo_states.shape[0])).T 1112 | E_mu_SA = np.zeros((1,S.shape[0]*len(ActionSet))) 1113 | for j in range(S.shape[0]): 1114 | E_mu_SA[0,j+S.shape[0]*int(round(policy_index_vector[j]))] = 1 # use the learned policy, unify the states 1115 | 1116 | T = T_count + 0 1117 | for j in range(T_count.shape[0]): # state 1118 | for k in range(T_count.shape[2]): #action 1119 | if sum(T_count[j,:,k]) == 0: 1120 | Aux = 1 1121 | else: 1122 | Aux = sum(T_count[j,:,k]) 1123 | T[j,:,k] = T_count[j,:,k]/Aux # normalized probability conditioned on S&A 1124 | 1125 | # policy is deterministic and unique 1126 | 1127 | # update nn.weights using MaxEnt 1128 | 1129 | nn.UpdWeiMaxEnt(S, ActionSet, mu_D_SA_unify, E_mu_SA, 0.005, 0, -0.0001) 1130 | print('State-Action Visitation Count Error: {:f}'.format((mu_D_SA_unify-E_mu_SA).dot((mu_D_SA_unify-E_mu_SA).T)[0][0])) 1131 | print('========== Epoch {:d} end =========='.format(n+1)) 1132 | 1133 | for i in range(len(nn.weights)): 1134 | #================================== Mac OS===================================== 1135 | np.save('IRL_temp_folder/save_reward_fcn{}.npy'.format(i),nn.weights[i]) 1136 | #================================= Windows ==================================== 1137 | # np.save('IRL_temp_folder\save_reward_fcn{}.npy'.format(i),nn.weights[i]) 1138 | #============================================================================== 1139 | 1140 | if (mu_D_SA_unify-E_mu_SA).dot((mu_D_SA_unify-E_mu_SA).T)[0][0]<2: 1141 | break 1142 | #========================== save final train result ========================== 1143 | 1144 | #================================== Mac OS===================================== 1145 | np.savetxt('IRL_temp_folder/save_GenerateDemos_MinState_320_left.txt', HV.Q_value[:,:,0], fmt='%1.4e',newline='\r\n') 1146 | np.savetxt('IRL_temp_folder/save_GenerateDemos_MinState_320_mid.txt', HV.Q_value[:,:,1], fmt='%1.4e',newline='\r\n') 1147 | np.savetxt('IRL_temp_folder/save_GenerateDemos_MinState_320_right.txt', HV.Q_value[:,:,2], fmt='%1.4e',newline='\r\n') 1148 | 1149 | np.savetxt('IRL_temp_folder/save_optimal_policy.txt', policy_index, fmt='%d',newline='\r\n') 1150 | np.savetxt('animation/save_optimal_policy.txt', policy_index, fmt='%d',newline='\r\n') 1151 | #================================= Windows ==================================== 1152 | # np.savetxt('IRL_temp_folder\save_GenerateDemos_MinState_320_left.txt', HV.Q_value[:,:,0], fmt='%1.4e',newline='\r\n') 1153 | # np.savetxt('IRL_temp_folder\save_GenerateDemos_MinState_320_mid.txt', HV.Q_value[:,:,1], fmt='%1.4e',newline='\r\n') 1154 | # np.savetxt('IRL_temp_folder\save_GenerateDemos_MinState_320_right.txt', HV.Q_value[:,:,2], fmt='%1.4e',newline='\r\n') 1155 | # 1156 | # np.savetxt('IRL_temp_folder\save_optimal_policy.txt', policy_index, fmt='%d',newline='\r\n') 1157 | # np.savetxt('animation\save_optimal_policy.txt', policy_index, fmt='%d',newline='\r\n') 1158 | #============================================================================== -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | This is a python code that implements the algorithm introduced in the paper "Advanced Planning for Autonomous Vehicles using Reinforcement Learning and Deep Inverse Reinforcement Learning" by Changxi You, Jianbo Lu, Dimitar Filev, and Panagiotis Tsiotras published in Robotics and Autonomous Systems, Vol. 114, pp. 1-18, 2019, doi:10.1016/j.robot.2019.01.003 2 | 3 | the code is written in Pyhton3. In Windows you can install Miniconda and then launch the simulator AutonomousCarTruckCurveRoadControl.py 4 | 5 | Some dependencies might be required when you compile the file. You can figure out what is missing from the prompt. During the implementation of the simulalor, one can start/stop capturing the video screenshots by click the 'v' key on the keyboard. One then could generate a smooth video using the screenshots in the ScreenFolder with the ffmpeg toolbox. 6 | -------------------------------------------------------------------------------- /save_optimal_policy.txt: -------------------------------------------------------------------------------- 1 | 1 1 1 2 | 1 1 1 3 | 1 1 1 4 | 1 1 1 5 | 1 1 1 6 | 1 1 1 7 | 1 1 1 8 | 1 1 1 9 | 1 1 1 10 | 1 1 1 11 | 1 1 1 12 | 1 1 1 13 | 1 1 1 14 | 1 1 1 15 | 1 1 1 16 | 1 1 1 17 | 0 0 4 18 | 0 0 4 19 | 0 4 4 20 | 0 0 4 21 | 0 0 0 22 | 0 0 0 23 | 0 0 0 24 | 0 0 0 25 | 0 0 4 26 | 0 0 4 27 | 0 0 4 28 | 0 0 4 29 | 0 0 0 30 | 0 0 0 31 | 0 0 0 32 | 0 0 0 33 | 1 1 1 34 | 1 1 1 35 | 1 1 1 36 | 1 1 1 37 | 1 1 1 38 | 1 1 1 39 | 1 1 1 40 | 1 1 1 41 | 1 1 1 42 | 1 1 1 43 | 1 1 1 44 | 1 1 1 45 | 1 1 1 46 | 1 1 1 47 | 1 1 1 48 | 1 1 1 49 | 1 1 1 50 | 1 1 1 51 | 1 1 1 52 | 1 1 1 53 | 1 1 1 54 | 1 1 1 55 | 1 1 1 56 | 1 1 1 57 | 1 1 1 58 | 1 1 1 59 | 1 1 1 60 | 1 1 1 61 | 1 1 1 62 | 1 1 1 63 | 1 1 1 64 | 1 1 1 65 | 1 1 1 66 | 1 1 1 67 | 1 1 1 68 | 1 1 1 69 | 1 1 1 70 | 1 1 1 71 | 1 1 1 72 | 1 1 1 73 | 1 1 1 74 | 1 1 1 75 | 1 1 1 76 | 1 1 1 77 | 1 1 1 78 | 1 1 1 79 | 1 1 1 80 | 1 1 1 81 | 1 1 1 82 | 1 1 1 83 | 1 1 1 84 | 1 1 1 85 | 1 1 1 86 | 1 1 1 87 | 1 1 1 88 | 1 1 1 89 | 1 1 1 90 | 1 1 1 91 | 1 1 1 92 | 1 1 1 93 | 1 1 1 94 | 1 1 1 95 | 1 1 1 96 | 1 1 1 97 | 3 0 4 98 | 3 0 4 99 | 3 0 4 100 | 3 0 4 101 | 3 0 4 102 | 3 0 4 103 | 3 0 4 104 | 0 0 4 105 | 3 0 0 106 | 3 0 0 107 | 3 0 0 108 | 3 0 0 109 | 3 0 0 110 | 3 0 0 111 | 3 0 0 112 | 3 0 0 113 | 4 0 4 114 | 0 0 4 115 | 0 0 4 116 | 0 0 4 117 | 0 0 4 118 | 0 0 4 119 | 0 0 4 120 | 0 0 4 121 | 0 0 0 122 | 0 0 0 123 | 0 0 0 124 | 0 0 0 125 | 0 0 0 126 | 0 0 0 127 | 0 0 0 128 | 0 0 0 129 | 3 0 4 130 | 3 0 0 131 | 3 0 0 132 | 3 0 4 133 | 3 0 4 134 | 3 0 4 135 | 3 0 4 136 | 3 0 4 137 | 3 0 0 138 | 3 0 0 139 | 3 0 0 140 | 3 0 0 141 | 3 0 0 142 | 3 0 0 143 | 3 0 0 144 | 3 0 0 145 | 0 0 0 146 | 0 0 4 147 | 0 0 4 148 | 0 0 0 149 | 0 0 0 150 | 0 0 0 151 | 0 0 0 152 | 0 0 4 153 | 0 0 0 154 | 0 0 0 155 | 0 0 0 156 | 0 0 0 157 | 0 0 0 158 | 0 0 0 159 | 0 0 0 160 | 0 0 0 161 | 1 1 1 162 | 1 1 1 163 | 1 1 1 164 | 1 1 1 165 | 1 1 1 166 | 1 1 1 167 | 1 1 1 168 | 1 1 1 169 | 1 1 1 170 | 1 1 1 171 | 1 1 1 172 | 1 1 1 173 | 1 1 1 174 | 1 1 1 175 | 1 1 1 176 | 1 1 1 177 | 1 1 1 178 | 1 1 1 179 | 1 1 1 180 | 1 1 1 181 | 1 1 1 182 | 1 1 1 183 | 1 1 1 184 | 1 1 1 185 | 1 1 1 186 | 1 1 1 187 | 1 1 1 188 | 1 1 1 189 | 1 1 1 190 | 1 1 1 191 | 1 1 1 192 | 1 1 1 193 | 1 1 1 194 | 1 1 1 195 | 1 1 1 196 | 1 1 1 197 | 1 1 1 198 | 1 1 1 199 | 1 1 1 200 | 1 1 1 201 | 1 1 1 202 | 1 1 1 203 | 1 1 1 204 | 1 1 1 205 | 1 1 1 206 | 1 1 1 207 | 1 1 1 208 | 1 1 1 209 | 1 1 1 210 | 1 1 1 211 | 1 1 1 212 | 1 1 1 213 | 1 1 1 214 | 1 1 1 215 | 1 1 1 216 | 1 1 1 217 | 1 1 1 218 | 1 1 1 219 | 1 1 1 220 | 1 1 1 221 | 1 1 1 222 | 1 1 1 223 | 1 1 1 224 | 1 1 1 225 | 0 0 4 226 | 0 0 4 227 | 0 0 4 228 | 3 0 4 229 | 3 0 4 230 | 0 0 4 231 | 3 0 4 232 | 3 0 4 233 | 0 0 0 234 | 0 0 0 235 | 0 0 0 236 | 3 0 0 237 | 0 0 0 238 | 3 0 0 239 | 0 0 0 240 | 0 0 0 241 | 0 0 4 242 | 0 0 4 243 | 0 0 4 244 | 0 0 4 245 | 0 0 4 246 | 0 0 4 247 | 0 0 0 248 | 0 0 4 249 | 0 0 0 250 | 0 0 0 251 | 0 0 0 252 | 0 0 0 253 | 0 0 0 254 | 0 0 0 255 | 0 0 0 256 | 0 0 0 257 | 0 0 4 258 | 3 0 4 259 | 0 0 4 260 | 3 0 4 261 | 3 0 0 262 | 0 0 4 263 | 0 0 4 264 | 0 0 4 265 | 0 0 0 266 | 3 0 0 267 | 3 0 0 268 | 3 0 0 269 | 3 0 0 270 | 3 0 0 271 | 0 0 0 272 | 0 0 0 273 | 0 0 4 274 | 0 0 0 275 | 0 0 4 276 | 0 0 4 277 | 0 0 4 278 | 0 0 4 279 | 0 0 0 280 | 0 0 4 281 | 0 0 0 282 | 0 0 0 283 | 0 0 0 284 | 0 0 0 285 | 0 0 0 286 | 0 0 0 287 | 0 0 0 288 | 0 0 0 289 | 1 1 1 290 | 1 1 1 291 | 1 1 1 292 | 1 1 1 293 | 1 1 1 294 | 1 1 1 295 | 1 1 1 296 | 1 1 1 297 | 3 0 0 298 | 3 0 0 299 | 3 0 0 300 | 3 0 0 301 | 0 0 0 302 | 0 0 0 303 | 0 0 0 304 | 0 0 0 305 | 1 1 1 306 | 1 1 1 307 | 1 1 1 308 | 1 1 1 309 | 1 1 1 310 | 1 1 1 311 | 1 1 1 312 | 1 1 1 313 | 3 0 0 314 | 3 0 0 315 | 0 0 0 316 | 3 0 0 317 | 0 0 0 318 | 0 0 0 319 | 0 0 0 320 | 0 0 0 321 | -------------------------------------------------------------------------------- /video.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jun 5 08:38:26 2017 4 | 5 | @author: CYOU7 6 | """ 7 | 8 | import pygame 9 | pygame.init() 10 | def make_video(screen): 11 | _image_num = 0 12 | while True: 13 | _image_num += 1 14 | str_num = "0000" + str(_image_num) 15 | file_name = "image" + str_num[-5:] + ".jpg" 16 | pygame.image.save(screen, "ScreenFolder/"+file_name) 17 | # print("In generator ", file_name) # delete, just for demonstration 18 | # pygame.time.wait(1000) # delete, just for demonstration 19 | yield 20 | --------------------------------------------------------------------------------