def main():
# ======================================================================================================================
# ============================================= Initialize parameters =================================================
# ======================================================================================================================
N = 14 # Horizon length
n = 6; d = 2 # State and Input dimension
x0 = np.array([0.5, 0, 0, 0, 0, 0]) # Initial condition
xS = [x0, x0]
dt = 0.1
map = Map(0.4) # Initialize map
vt = 0.8 # target vevlocity
# Initialize controller parameters
mpcParam, ltvmpcParam = initMPCParams(n, d, N, vt)
numSS_it, numSS_Points, Laps, TimeLMPC, QterminalSlack, lmpcParameters = initLMPCParams(map, N)
# Init simulators
simulator = Simulator(map)
LMPCsimulator = Simulator(map, multiLap = False, flagLMPC = True)
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
print("Starting PID")
# Initialize pid and run sim
PIDController = PID(vt)
xPID_cl, uPID_cl, xPID_cl_glob, _ = simulator.sim(xS, PIDController)
print("===== PID terminated")
# ======================================================================================================================
# ====================================== LINEAR REGRESSION ============================================================
# ======================================================================================================================
print("Starting MPC")
# Estimate system dynamics
lamb = 0.0000001
A, B, Error = Regression(xPID_cl, uPID_cl, lamb)
mpcParam.A = A
mpcParam.B = B
# Initialize MPC and run closed-loop sim
mpc = MPC(mpcParam)
xMPC_cl, uMPC_cl, xMPC_cl_glob, _ = simulator.sim(xS, mpc)
print("===== MPC terminated")
# ======================================================================================================================
# =================================== LOCAL LINEAR REGRESSION =========================================================
# ======================================================================================================================
print("Starting TV-MPC")
# Initialized predictive model
predictiveModel = PredictiveModel(n, d, map, 1)
predictiveModel.addTrajectory(xPID_cl,uPID_cl)
#Initialize TV-MPC
ltvmpcParam.timeVarying = True
mpc = MPC(ltvmpcParam, predictiveModel)
# Run closed-loop sim
xTVMPC_cl, uTVMPC_cl, xTVMPC_cl_glob, _ = simulator.sim(xS, mpc)
print("===== TV-MPC terminated")
# ======================================================================================================================
# ============================== LMPC w\ LOCAL LINEAR REGRESSION ======================================================
# ======================================================================================================================
print("Starting LMPC")
# Initialize Predictive Model for lmpc
lmpcpredictiveModel = PredictiveModel(n, d, map, 4)
for i in range(0,4): # add trajectories used for model learning
lmpcpredictiveModel.addTrajectory(xPID_cl,uPID_cl)
# Initialize Controller
lmpcParameters.timeVarying = True
lmpc = LMPC(numSS_Points, numSS_it, QterminalSlack, lmpcParameters, lmpcpredictiveModel)
for i in range(0,4): # add trajectories for safe set
lmpc.addTrajectory( xPID_cl, uPID_cl, xPID_cl_glob)
# Run sevaral laps
for it in range(numSS_it, Laps):
# Simulate controller
xLMPC, uLMPC, xLMPC_glob, xS = LMPCsimulator.sim(xS, lmpc)
# Add trajectory to controller
lmpc.addTrajectory( xLMPC, uLMPC, xLMPC_glob)
# lmpcpredictiveModel.addTrajectory(np.append(xLMPC,np.array([xS[0]]),0),np.append(uLMPC, np.zeros((1,2)),0))
lmpcpredictiveModel.addTrajectory(xLMPC,uLMPC)
print("Completed lap: ", it, " in ", np.round(lmpc.Qfun[it][0]*dt, 2)," seconds")
print("===== LMPC terminated")
# # ======================================================================================================================
# # ========================================= PLOT TRACK =================================================================
# # ======================================================================================================================
for i in range(0, lmpc.it):
print("Lap time at iteration ", i, " is ",np.round( lmpc.Qfun[i][0]*dt, 2), "s")
print("===== Start Plotting")
plotTrajectory(map, xPID_cl, xPID_cl_glob, uPID_cl, 'PID')
plotTrajectory(map, xMPC_cl, xMPC_cl_glob, uMPC_cl, 'MPC')
plotTrajectory(map, xTVMPC_cl, xTVMPC_cl_glob, uTVMPC_cl, 'TV-MPC')
plotClosedLoopLMPC(lmpc, map)
animation_xy(map, lmpc, Laps-1)
plt.show()
# ======================================================================================================================
dt = 1.0 / 10.0 # Controller discretization time
Time = 100 # Simulation time for path following PID
TimeMPC = 100 # Simulation time for path following MPC
TimeMPC_tv = 100 # Simulation time for path following LTV-MPC
vt = 0.8 # Reference velocity for path following controllers
v0 = 0.5 # Initial velocity at lap 0
N = 12 # Horizon length
n = 6
d = 2 # State and Input dimension
# Path Following tuning
Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey
R = np.diag([1.0, 10.0]) # delta, a
map = Map(0.8) # Initialize the map (PointAndTangent); argument is track width
simulator = Simulator(map) # Initialize the Simulator
# ======================================================================================================================
# ==================================== Initialize parameters for LMPC ==================================================
# ======================================================================================================================
TimeLMPC = 400 # Simulation time
Laps = 5 + 2 # Total LMPC laps
# Safe Set Parameter
LMPC_Solver = "CVX" # Can pick CVX for cvxopt or OSQP. For OSQP uncomment line 14 in LMPC.py
numSS_it = 2 # Number of trajectories used at each iteration to build the safe set
numSS_Points = 32 + N # Number of points to select from each trajectory to build the safe set
shift = 0 # Given the closed point, x_t^j, to the x(t) select the SS points from x_{t+shift}^j
# Tuning Parameters
# ====================================================================================================================== dt = 1.0 / 10.0 # Controller discretization time Time = 100 # Simulation time for PID TimeMPC = 100 # Simulation time for path following MPC TimeMPC_tv = 100 # Simulation time for path following LTV-MPC vt = 0.8 # Reference velocity for path following controllers v0 = 0.5 # Initial velocity at lap 0 N = 12 # Horizon length n = 6 d = 2 # State and Input dimension # Path Following tuning Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey R = np.diag([1.0, 10.0]) # delta, a map = Map(0.4, 1) # Initialize the map simulator = Simulator(map) # Initialize the Simulator # ====================================================================================================================== # ==================================== Initialize parameters for LMPC ================================================== # ====================================================================================================================== TimeLMPC = 400 # Simulation time Laps = 25 + 2 # Total LMPC laps # Safe Set Parameters LMPC_Solver = "CVX" # Can pick CVX for cvxopt or OSQP. For OSQP uncomment line 14 in LMPC.py numSS_it = 2 # Number of trajectories used at each iteration to build the safe set numSS_Points = 40 # Number of points to select from each trajectory to build the safe set # Tuning Parameters Qslack = 2 * 5 * np.diag([
# ======================================================================================================================
dt = 1.0 / 10.0 # Controller discretization time
Time = 100 # Simulation time for PID
TimeMPC = 100 # Simulation time for path following MPC
TimeMPC_tv = 100 # Simulation time for path following LTV-MPC
vt = 0.8 # Reference velocity for path following controllers
v0 = 0.5 # Initial velocity at lap 0
N = 12 # Horizon length
n = 6
d = 2 # State and Input dimension
# Path Following tuning
Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey
R = np.diag([1.0, 10.0]) # delta, a
map = Map(0.8) # Initialize the map
simulator = Simulator(map) # Initialize the Simulator
# ======================================================================================================================
# ==================================== Initialize parameters for LMPC ==================================================
# ======================================================================================================================
TimeLMPC = 400 # Simulation time
Laps = 5 + 2 # Total LMPC laps
# Safe Set Parameter
LMPC_Solver = "OSQP" # Can pick CVX for cvxopt or OSQP. For OSQP uncomment line 14 in LMPC.py
numSS_it = 2 # Number of trajectories used at each iteration to build the safe set
numSS_Points = 32 + N # Number of points to select from each trajectory to build the safe set
shift = 0 # Given the closed point, x_t^j, to the x(t) select the SS points from x_{t+shift}^j
# Tuning Parameters
# ======================================================================================================================
dt = 1.0 / 10.0 # Controller discretization time
Time = 100 # Simulation time for path following PID
TimeMPC = 100 # Simulation time for path following MPC
TimeMPC_tv = 100 # Simulation time for path following LTV-MPC
vt = 0.8 # Reference velocity for path following controllers
v0 = 0.5 # Initial velocity at lap 0
N = 12 # Horizon length (12)
n = 6
d = 2 # State and Input dimension
# Path Following tuning
Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey
R = np.diag([1.0, 10.0]) # delta, a
map = Map(0.8) # Initialize the map (PointAndTangent); argument is track width
simulator = Simulator(map) # Initialize the Simulator
# ======================================================================================================================
# ==================================== Initialize parameters for LMPC ==================================================
# ======================================================================================================================
TimeLMPC = 400 # Simulation time
Laps = 5 + 2 # Total LMPC laps
# Safe Set Parameter
LMPC_Solver = "CVX" # Can pick CVX for cvxopt or OSQP. For OSQP uncomment line 14 in LMPC.py
numSS_it = 2 # Number of trajectories used at each iteration to build the safe set
numSS_Points = 32 + N # Number of points to select from each trajectory to build the safe set
shift = 0 # Given the closed point, x_t^j, to the x(t) select the SS points from x_{t+shift}^j
# Tuning Parameters
# ====================================================================================================================== dt = 1.0 / 10.0 # Controller discretization time Time = 100 # Simulation time for PID TimeMPC = 100 # Simulation time for path following MPC TimeMPC_tv = 100 # Simulation time for path following LTV-MPC vt = 0.8 # Reference velocity for path following controllers v0 = 0.5 # Initial velocity at lap 0 N = 12 # Horizon length n = 6 d = 2 # State and Input dimension # Path Following tuning Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey R = np.diag([1.0, 10.0]) # delta, a map = Map(0.4) # Initialize the map simulator = Simulator(map) # Initialize the Simulator # ====================================================================================================================== # ==================================== Initialize parameters for LMPC ================================================== # ====================================================================================================================== TimeLMPC = 4000 # Simulation time Laps = 40 + 2 # Total LMPC laps # Safe Set Parameters LMPC_Solver = "CVX" # Can pick CVX for cvxopt or OSQP. For OSQP uncomment line 14 in LMPC.py numSS_it = 2 # Number of trajectories used at each iteration to build the safe set numSS_Points = 40 # Number of points to select from each trajectory to build the safe set # Tuning Parameters Qslack = 2 * 5 * np.diag([
def main():
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(2.0)
world = client.get_world()
settings = world.get_settings()
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)
# pygame.init()
# display = pygame.display.set_mode(
# (800, 600),
# pygame.HWSURFACE | pygame.DOUBLEBUF)
# font = get_font()
# clock = pygame.time.Clock()
# spectator = world.get_spectator()
maxLaps = 10
fps = 20
Flag = 0
lat_width = 640
lat_height = 360
cam_height = 0.6 #ought to be 0.5m
cam_angle = -10
cam_offset = 1.06
blind_zone = 0.4 + cam_offset #n meters
halfwidth = 3.7
count = 0
store = None
dt = 0.05
try:
blueprint_library = world.get_blueprint_library()
bp = blueprint_library.filter('model3')[0]
# spawn_point = random.choice(world.get_map().get_spawn_points())
loc = carla.Location(x=-2.99717595, y=-87.820107, z=0.30)
rot = carla.Rotation(pitch=0.0, yaw=90.47937, roll=0.0)
tran = carla.Transform(loc, rot)
vehicle = world.spawn_actor(bp, tran)
# vehicle.set_simulate_physics(False)
#Camera Sensor
bp = world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', f'{lat_width}')
bp.set_attribute('image_size_y', f'{lat_height}')
bp.set_attribute('fov', '110')
lc_cam = world.spawn_actor(bp,
carla.Transform(
carla.Location(y=-cam_offset,
z=cam_height),
carla.Rotation(pitch=cam_angle,
yaw=-90.0)),
attach_to=vehicle)
#TrailingCamera
trail_cam = world.spawn_actor(
blueprint_library.find('sensor.camera.rgb'),
carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15)),
attach_to=vehicle)
#IMU Sensor
bp = world.get_blueprint_library().find('sensor.other.imu')
imu = world.spawn_actor(bp, carla.Transform(), attach_to=vehicle)
#GNSS Sensor
bp = world.get_blueprint_library().find('sensor.other.gnss')
gnss = world.spawn_actor(bp, carla.Transform(), attach_to=vehicle)
#Teleport vehicle to new location
#Initialize sensor objects
IMU = IMUSensor(imu)
CAM = CamManager(cam_height, cam_angle, blind_zone, halfwidth)
GPS = GNSSSensor(gnss)
track_dat = np.load('pnt_curv.npy')
map = Map(3.40, track_dat)
phys = vehicle.get_physics_control()
lmpc = RaceLMPC(dt, 60, maxLaps, map, phys)
max_steer = phys.wheels[0].max_steer_angle
world.tick()
count = 0
lap_dat = np.zeros((30000, 9))
# world_snapshot = world.wait_for_tick()
# actor_snapshot = world_snapshot.find(vehicle.id)
## Set spectator at given transform (vehicle transform)
# spectator.set_transform(actor_snapshot.get_transform())
# time.sleep(2)
with commander(world, maxLaps, fps, vehicle, imu, lc_cam, trail_cam,
gnss, Map, lmpc, max_steer) as controller:
while Flag == 0:
# clock.tick()
snap, imu_dat, cam_dat, t_dat, gps_dat = controller.tick(
timeout=5.0)
accel, gyro = IMU.Data(imu_dat)
lat, long = GPS.Data(gps_dat)
#Processing for Image data
angle, dist = CAM.lateral_imgproc(cam_dat)
epsi, deviation = CAM.Data(angle, dist)
Flag, lap, LapTrigger, lmpc, LapTime, x, x_glob, predist = controller.lap_record(
snap, gyro, deviation, epsi, lmpc)
lap_dat[count, 0:1] = x[0:1]
lap_dat[count, 2] = x[4]
lap_dat[count, 3:4] = x_glob[4:5]
lap_dat[count, 5] = accel[1]
lap_dat[count, 6] = LapTime
lap_dat[count, 7] = lap
lap_dat[count, 8] = predist
if lap > 0:
#Apply new control
new_v = carla.Vector3D(x=x[0] * 3.6, y=x[1] * 3.6)
new_om = carla.Vector3D(z=x[2])
vehicle.set_velocity(new_v)
vehicle.set_angular_velocity(new_om)
if LapTrigger == 1:
if lap == 0:
lmpc.PIDinit(x)
elif lap == 1:
lmpc.LTIinit(x, LapTime)
elif lap == 2:
lmpc.PIDinit(x)
elif lap == 3:
lmpc.LTVinit(x, LapTime)
elif lap == 4:
lmpc.LMPCinit(x, LapTime)
count += 1
# fpsn = round(1.0 / snap.timestamp.delta_seconds)
# # Draw the display.
# draw_image(display, t_dat)
# display.blit(
# font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)),
# (8, 10))
# display.blit(
# font.render('% 5d FPS (simulated)' % fpsn, True, (255, 255, 255)),
# (8, 28))
# pygame.display.flip()
finally:
vehicle.destroy()
print('We are Done!')
lap_dat = np.delete(lap_dat, slice(count - 1, 29999), axis=0)
np.save('Lap_data', lap_dat)
pygame.quit()