├── MPC_carla_report.pdf ├── README.md └── mpc.py /MPC_carla_report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rajshah05/Model_Predictive_Controller/HEAD/MPC_carla_report.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Model_Predictive_Controller (MPC) 2 | Optimally navigated an autonomous vehicle using MPC in the CARLA simulator 3 | 4 | # How to run 5 | 1. Download and install carla from http://carla.org/ 6 | 2. download and unzip the repository 7 | 3. copy paste the mpc.py file in carla\WindowsNoEditor\PythonAPI\examples 8 | 4. Run the mpc.py file 9 | 10 | # Please refer "MPC_carla_report.pdf" file for implementation and project details 11 | 12 | # Video link: 13 | https://www.youtube.com/watch?v=tBYQyAXvXEg&feature=youtu.be 14 | -------------------------------------------------------------------------------- /mpc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import glob 4 | import os 5 | import sys 6 | import random 7 | import time 8 | import numpy as np 9 | import cv2 10 | from test import * 11 | 12 | 13 | try: 14 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 15 | sys.version_info.major, 16 | sys.version_info.minor, 17 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 18 | except IndexError: 19 | pass 20 | 21 | 22 | import carla 23 | 24 | IM_WIDTH = 640 25 | IM_HEIGHT = 480 26 | 27 | 28 | 29 | actor_list = [] 30 | 31 | try: 32 | client = carla.Client("localhost",2000) 33 | client.set_timeout(10.0) 34 | world = client.get_world() 35 | blueprint_library = world.get_blueprint_library() 36 | 37 | vehicle_bp = blueprint_library.filter("model3")[0] 38 | 39 | startpoint = world.get_map().get_spawn_points()[195] #128 195 40 | world.debug.draw_string(startpoint.location, 'O', draw_shadow=False, 41 | color=carla.Color(r=255, g=0, b=0), life_time=50, 42 | persistent_lines=True) 43 | 44 | vehicle = world.spawn_actor(vehicle_bp, startpoint) 45 | # ---------------------Trajectory----------------- 46 | wplist = world.get_map().get_topology() 47 | wps = wplist[270][0].next_until_lane_end(5.0) # 22 (195:270) 48 | for w in wps: 49 | world.debug.draw_string(w.transform.location, 'O', draw_shadow=False, 50 | color=carla.Color(r=0, g=255, b=0), life_time=20.0, 51 | persistent_lines=True) 52 | 53 | endpoint = wps[0].transform 54 | 55 | # -----------X-ref-------------------- 56 | # cvel = norm_2([vehicle.get_velocity().x, vehicle.get_velocity().y]) 57 | # ttnwp = 2/cvel 58 | 59 | 60 | # --------------------trial end --------------- 61 | # nwp = wps[0].next(2.0) 62 | # for w in nwp: 63 | # world.debug.draw_string(w.transform.location, 'O', draw_shadow=False, 64 | # color=carla.Color(r=0, g=0, b=255), life_time=20.0, 65 | # persistent_lines=True) 66 | 67 | # -----------------curvature---------------- 68 | # endpoint = world.get_map().get_waypoint(carla.Location(x=-65.7, y=188.7, z=0.0), project_to_road=True, lane_type=carla.LaneType.Driving).transform 69 | # print(endpoint) 70 | # world.debug.draw_string(endpoint.location, 'O', draw_shadow=False, 71 | # color=carla.Color(r=255, g=0, b=0), life_time=20, 72 | # persistent_lines=True) 73 | 74 | 75 | # ------------------straight------------------- 76 | # endpoint = world.get_map().get_waypoint(carla.Location(x=-53.375589, y=197.064423, z=1.843102), project_to_road=True, lane_type=carla.LaneType.Driving).transform 77 | # print(endpoint) 78 | # world.debug.draw_string(endpoint.location, 'O', draw_shadow=False, 79 | # color=carla.Color(r=255, g=0, b=0), life_time=50, 80 | # persistent_lines=True) 81 | 82 | 83 | actor_list.append(vehicle) 84 | 85 | # -------------------MPC-------------------- 86 | 87 | 88 | # SAMPLING TIME 89 | T = 0.08 #0.08 # (s) 90 | 91 | # PREDICTION HORIZON 92 | N = 12 # 12 5 93 | 94 | # STATES 95 | x = SX.sym('x') # x coordinate 96 | y = SX.sym('y') # y coordinate 97 | theta = SX.sym('theta') # vehicle orientation 98 | v = SX.sym('v') # longitudenal velocity 99 | states = vertcat(x,y,theta,v) 100 | n_states = 4 # no. of states 101 | 102 | # CONTROL 103 | thr = SX.sym('thr') # Throttle 104 | strang = SX.sym('strang') # steering angle 105 | controls = vertcat(thr,strang) 106 | n_controls = 2 107 | 108 | # CONTROL BOUNDS 109 | minthr = 0.0 # minimum throttle 110 | maxthr = 0.5 # maximum throttle 111 | minstrang = -1 # minimum steering angle 112 | maxstrang = 1 # maximum steering angle 113 | 114 | # VEHICLE MODEL PARAMETERS 115 | l_r = 1.415 # distance from center of gravity to rare wheels 116 | l_f = 1.6# distance from center of graity to front wheels 117 | 118 | # SYMBOLIC REPRESENTATION OF THE DERIVATIVE OF THE STATES BASED ON THE BICYCE MODEL 119 | rhs = vertcat((v*cos(theta+(atan((l_r*tan(strang*1.22))/(l_f + l_r))))), v*sin(theta+(atan((l_r*tan(strang*1.22))/(l_f + l_r)))), ((v/l_r)*(sin(atan((l_r*tan(strang*1.22))/(l_f + l_r))))), thr*16) 120 | 121 | # STATE PREDICTION - SYMBOLIC FUNCTION OF CURRENT STATE AND CONTROL INPUTS AT EACH TIME STEP OF HORIZON PERIOD 122 | f = Function('f', [states, controls], [rhs]) 123 | U = SX.sym('U', n_controls, N) 124 | P = SX.sym('P', n_states + n_states) 125 | X = SX.sym('X', n_states, (N+1)) 126 | X[:,0] = P[0:4] 127 | 128 | for k in range(N): 129 | st = X[:,k] 130 | con = U[:,k] 131 | f_value = f(st, con) 132 | st_next = st + (T*f_value) 133 | X[:,k+1] = st_next 134 | ff = Function('ff', [U,P], [X]) 135 | 136 | # SYBOLIC REPRESENTATION OF THE OBJECTIVE FUNCTION 137 | obj = 0 138 | g=SX.sym('g',4,(N+1)) 139 | Q = diag(SX([3600,3600,1900,2])) #195 [3600,3600,1900,2] [3100,3100,1900,2] [2700,2700,2000,2] 140 | R = diag(SX([0,8000])) #195 [0,7000] 141 | 142 | # Q = diag(SX([99999,99999,1000,2])) #128 [4000,4000,1000,2] 143 | # R = diag(SX([0,0])) #128 144 | 145 | for k in range(N): 146 | st = X[:,k] 147 | con = U[:,k] 148 | obj = obj + mtimes(mtimes((st - P[4:8]).T,Q), (st - P[4:8])) + mtimes(mtimes(con.T, R), con) 149 | 150 | # STATES BOUNDS/CONSTRAINTS 151 | for k in range(0,N+1): 152 | g[0,k] = X[0,k] 153 | g[1,k] = X[1,k] 154 | g[2,k] = X[2,k] 155 | g[3,k] = X[3,k] 156 | g = reshape(g, 4*(N+1), 1) 157 | 158 | 159 | # CREATING A OPTIMIZATION SOLVER IN CASADI 160 | OPT_variables = reshape(U, 2*N, 1) 161 | 162 | nlp_prob = {'f':obj, 'x':OPT_variables, 'g':g, 'p':P} 163 | 164 | opts = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6} 165 | 166 | solver = nlpsol('solver','ipopt', nlp_prob, opts) # solver 167 | 168 | # IMPLEMENTING CONTROL BOUNDS 169 | lbx = [] 170 | ubx = [] 171 | for i in range(2*N): 172 | if i%2==0: 173 | lbx.append(minthr) 174 | ubx.append(maxthr) 175 | else: 176 | lbx.append(minstrang) 177 | ubx.append(maxstrang) 178 | lbx = np.transpose(lbx) 179 | ubx = np.transpose(ubx) 180 | 181 | # IMPLEMENTING STATE BOUNDS 182 | lbgv = [] 183 | ubgv = [] 184 | for i in range(0,4*(N+1),4): 185 | lbgv.append(-300) 186 | lbgv.append(-300) 187 | lbgv.append(0) 188 | lbgv.append(0) 189 | ubgv.append(300) 190 | ubgv.append(300) 191 | ubgv.append(405) 192 | ubgv.append(15) 193 | # args = {'lbg':-99999, 'ubg':99999, 'lbx':lbxv, 'ubx':ubxv} 194 | u0 = (DM.zeros(2*N,1)) 195 | # sim_tim = 20 196 | u_cl = [] 197 | 198 | def contheta(thet): 199 | if thet < 0: 200 | thet = 360 - abs(thet) 201 | return thet 202 | 203 | # r_theta = startpoint.rotation.yaw 204 | # if r_theta < 0: 205 | # r_theta = 360 - abs(r_theta) 206 | 207 | # if endpoint.rotation.yaw < 0: 208 | # endpoint.rotation.yaw = 360 - abs(endpoint.rotation.yaw) 209 | 210 | # x0 = np.transpose([startpoint.location.x, startpoint.location.y, r_theta, 0]) 211 | # xs = np.transpose([endpoint.location.x, endpoint.location.y, r_theta, 0]) #-90.156235*pi/180 212 | # p = np.transpose([startpoint.location.x, startpoint.location.y, r_theta, 0, endpoint.location.x, endpoint.location.y, r_theta, 0]) 213 | 214 | 215 | x0 = np.transpose([startpoint.location.x, startpoint.location.y, contheta(startpoint.rotation.yaw), 0]) 216 | xs = np.transpose([endpoint.location.x, endpoint.location.y, contheta(startpoint.rotation.yaw), 3]) #-90.156235*pi/180 217 | 218 | c=0 219 | p = np.transpose([startpoint.location.x, startpoint.location.y, contheta(startpoint.rotation.yaw), 0,endpoint.location.x, endpoint.location.y, contheta(startpoint.rotation.yaw), 3]) 220 | mpciter = 0 221 | 222 | # MPC LOOP 223 | # while (norm_2(x0[0:2]-xs[0:2]))>0.5: 224 | while c < len(wps): 225 | # r_theta = startpoint.rotation.yaw 226 | # if r_theta < 0: 227 | # r_theta = 360 - abs(r_theta) 228 | 229 | # if endpoint.rotation.yaw < 0: 230 | # endpoint.rotation.yaw = 360 - abs(endpoint.rotation.yaw) 231 | # cvel = norm_2([vehicle.get_velocity().x, vehicle.get_velocity().y]) 232 | # ttnwp = 2/cvel 233 | # if mpciter > ttnwp/T: 234 | # print(c) 235 | # print(len(wps)) 236 | # endpoint = wps[c+1].transform 237 | # c +=1 238 | # print((norm_2(x0[0:2]-p[4:6]))) 239 | # print(c) 240 | if (norm_2(x0[0:2]-p[4:6]))<3: 241 | c += 1 242 | endpoint = wps[c].transform 243 | world.debug.draw_string(endpoint.location, 'O', draw_shadow=False, 244 | color=carla.Color(r=0, g=0, b=255), life_time=3, 245 | persistent_lines=True) 246 | 247 | 248 | 249 | 250 | 251 | print(x0,"---",p[4:8]) 252 | 253 | u0 = reshape(u0, 2*N,1) 254 | p[0:4] = x0 255 | p[4:8] = [endpoint.location.x, endpoint.location.y, contheta(endpoint.rotation.yaw), 3]#6 256 | 257 | 258 | sol = solver(x0=u0, lbx=lbx, ubx=ubx, lbg=lbgv, ubg=ubgv, p=p) 259 | 260 | u = reshape(sol['x'].T, 2, N).T 261 | ff_value = ff(u.T, p) 262 | # print(ff_value) 263 | for k in range(N): 264 | 265 | world.debug.draw_string(carla.Location(x=float(ff_value[0,k]), y=float(ff_value[1,k]),z=0.0), 'O', draw_shadow=False, 266 | color=carla.Color(r=255, g=0, b=0), life_time=0.01, 267 | persistent_lines=True) 268 | # xx1[:,0:2,mpciter] = ff_value.T 269 | u_cl.append(u[0,:]) 270 | # print("throttle",float(u[0,0]),"strang", float(u[0,1])) 271 | # print(u[0,0], u[0,1]) 272 | 273 | vehicle.apply_control(carla.VehicleControl(throttle =float(u[0,0]) , steer = float(u[0,1]))) 274 | # print(vehicle.get_transform().rotation.yaw) 275 | u_theta = vehicle.get_transform().rotation.yaw 276 | # if u_theta < 0: 277 | # u_theta = 360 - abs(u_theta) 278 | x0 = np.transpose([vehicle.get_transform().location.x, vehicle.get_transform().location.y, contheta(u_theta), norm_2([vehicle.get_velocity().x, vehicle.get_velocity().y])]) 279 | 280 | u0 = reshape(u0, N, 2) 281 | u0[0:N-1,:] = u[1:N,:] 282 | u0[N-1,:]=u[N-1,:] 283 | 284 | mpciter += 1 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | time.sleep(10) 296 | finally: 297 | for actor in actor_list: 298 | actor.destroy() 299 | print("All cleaned up!") --------------------------------------------------------------------------------