def __init__(self, dt, Time, maxlaps, map, physics):
# ======================================================================================================================
# ============================ Choose which controller to run ==========================================================
# ======================================================================================================================
# self.RunPID = 1
# self.RunMPC = 1
# self.RunMPC_tv = 1
# self.RunLMPC = 1
# ======================================================================================================================
# ============================ Initialize parameters for path following ================================================
# ======================================================================================================================
self.dt = dt # Controller discretization time
self.Time = 200 # Simulation time for PID
self.TimeMPC = None # Simulation time for path following MPC
self.TimeMPC_tv = None # Simulation time for path following LTV-MPC
self.vt = 40.0 # Reference velocity for path following controllers
self.v0 = None # Initial velocity at lap 0
self.N = 12 # Horizon length
self.n = 6; self.d = 2 # State and Input dimension
self.points = int(self.Time/dt)
self.count = 0
# self.PIDinit = 0
# self.LTIinit = 0
# self.LTVinit = 0
# self.LMPCinit = 0
# Path Following tuning
self.Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey
self.R = np.diag([1.0, 10.0]) # delta, a
#LTI-MPC Params
self.A = None
self.B = None
self.error = None
self.u = np.array([0., 0.], dtype=float)
self.map = map # Initialize the map
self.Dynamic = DynamicModel(physics)
self.simulator = Simulator(self.Dynamic, self.map) # Initialize the Simulator
# ======================================================================================================================
# ==================================== Initialize parameters for LMPC ==================================================
# ======================================================================================================================
# Safe Set Parameters
self.LMPC_Solver = "CVX" # Can pick CVX for cvxopt or OSQP. For OSQP uncomment line 14 in LMPC.py
self.numSS_it = 4 # Number of trajectories used at each iteration to build the safe set
self.numSS_Points = 20 # Number of points to select from each trajectory to build the safe set
self.Laps = maxlaps + self.numSS_it # Total LMPC laps
self.TimeLMPC = 400 # Simulation time
# Tuning Parameters
self.Qslack = 20 * np.diag([10, 1, 1, 1, 10, 1]) # Cost on the slack variable for the terminal constraint
self.Qlane = 1 * np.array([0, 10]) # Quadratic and linear slack lane cost
self.Q_LMPC = 0 * np.diag([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # State cost x = [vx, vy, wz, epsi, s, ey]
self.R_LMPC = 0 * np.diag([1.0, 1.0]) # Input cost u = [delta, a]
self.dR_LMPC = 10 * np.array([1.0, 10.0]) # Input rate cost u
self.inputConstr = np.array([[0.8, 0.8], # Min Steering and Max Steering
[10.0, 10.0]]) # Min Acceleration and Max Acceleration
def LMPCinit(self, x, LapTime):
print("Starting LMPC")
self.TimeLMPC = LapTime
self.points = int(self.TimeLMPC/self.dt)
self.v0 = x[0]
self.ClosedLoopLMPC = ClosedLoopData(self.points, self.v0)
self.LMPCOpenLoopData = LMPCprediction(self.N, self.n, self.d, self.TimeLMPC, self.numSS_Points, self.Laps)
self.LMPCSimulator = Simulator(self.Dynamic, map, 1, 1)
self.LMPController = ControllerLMPC(self.numSS_Points, self.numSS_it, self.N, self.Qslack, self.Qlane, self.Q_LMPC,
self.R_LMPC, self.dR_LMPC, self.dt, self.map, self.Laps, self.TimeLMPC, self.LMPC_Solver, self.inputConstr)
PID_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataPID.obj', 'rb')
ClosedLoopDataPID = pickle.load(PID_data)
LTV_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTV_MPC.obj','rb')
ClosedLoopDataLTV_MPC = pickle.load(LTV_data)
LTI_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTI_MPC.obj','rb')
ClosedLoopDataLTI_MPC = pickle.load(LTI_data)
# Run first 4 laps. Remaining laps run by LMPC
self.LMPController.addTrajectory(ClosedLoopDataPID)
self.LMPController.addTrajectory(ClosedLoopDataLTV_MPC)
self.LMPController.addTrajectory(ClosedLoopDataPID)
self.LMPController.addTrajectory(ClosedLoopDataLTI_MPC)
PID_data.close()
LTV_data.close()
LTI_data.close()
self.count = 0
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 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([
10, 1, 1, 1, 10, 1
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
Qslack = 5 * np.diag([
class RaceLMPC(object):
def __init__(self, dt, Time, maxlaps, map, physics):
# ======================================================================================================================
# ============================ Choose which controller to run ==========================================================
# ======================================================================================================================
# self.RunPID = 1
# self.RunMPC = 1
# self.RunMPC_tv = 1
# self.RunLMPC = 1
# ======================================================================================================================
# ============================ Initialize parameters for path following ================================================
# ======================================================================================================================
self.dt = dt # Controller discretization time
self.Time = 200 # Simulation time for PID
self.TimeMPC = None # Simulation time for path following MPC
self.TimeMPC_tv = None # Simulation time for path following LTV-MPC
self.vt = 40.0 # Reference velocity for path following controllers
self.v0 = None # Initial velocity at lap 0
self.N = 12 # Horizon length
self.n = 6; self.d = 2 # State and Input dimension
self.points = int(self.Time/dt)
self.count = 0
# self.PIDinit = 0
# self.LTIinit = 0
# self.LTVinit = 0
# self.LMPCinit = 0
# Path Following tuning
self.Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey
self.R = np.diag([1.0, 10.0]) # delta, a
#LTI-MPC Params
self.A = None
self.B = None
self.error = None
self.u = np.array([0., 0.], dtype=float)
self.map = map # Initialize the map
self.Dynamic = DynamicModel(physics)
self.simulator = Simulator(self.Dynamic, self.map) # Initialize the Simulator
# ======================================================================================================================
# ==================================== Initialize parameters for LMPC ==================================================
# ======================================================================================================================
# Safe Set Parameters
self.LMPC_Solver = "CVX" # Can pick CVX for cvxopt or OSQP. For OSQP uncomment line 14 in LMPC.py
self.numSS_it = 4 # Number of trajectories used at each iteration to build the safe set
self.numSS_Points = 20 # Number of points to select from each trajectory to build the safe set
self.Laps = maxlaps + self.numSS_it # Total LMPC laps
self.TimeLMPC = 400 # Simulation time
# Tuning Parameters
self.Qslack = 20 * np.diag([10, 1, 1, 1, 10, 1]) # Cost on the slack variable for the terminal constraint
self.Qlane = 1 * np.array([0, 10]) # Quadratic and linear slack lane cost
self.Q_LMPC = 0 * np.diag([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # State cost x = [vx, vy, wz, epsi, s, ey]
self.R_LMPC = 0 * np.diag([1.0, 1.0]) # Input cost u = [delta, a]
self.dR_LMPC = 10 * np.array([1.0, 10.0]) # Input rate cost u
self.inputConstr = np.array([[0.8, 0.8], # Min Steering and Max Steering
[10.0, 10.0]]) # Min Acceleration and Max Acceleration
def PIDinit(self, x):
self.v0 = x[0]
self.ClosedLoopDataPID = ClosedLoopData(self.points , self.v0)
self.PIDController = PID(self.vt)
print("Starting PID")
self.count = 0
def PIDcont(self, x, x_glob, LapTime, SimTime, EndFlag=0):
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
if EndFlag == 0:
u, new_x, new_x_glob = self.simulator.Sim(x, x_glob, SimTime, self.PIDController)
self.ClosedLoopDataPID.UpdateLoop(self.count, x, x_glob, u, SimTime)
self.count += 1
return new_x, new_x_glob
else:
file_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataPID.obj', 'wb')
pickle.dump(self.ClosedLoopDataPID, file_data)
file_data.close()
print("===== PID terminated")
self.PIDinit = 0 #It will run 2nd time
print("Starting LTI-MPC")
lamb = 0.0000001
self.A, self.B, self.Error = Regression(self.ClosedLoopDataPID.x, self.ClosedLoopDataPID.u, lamb)
return new_x, new_x_glob
def LTIinit(self, x, LapTime):
self.TimeMPC = LapTime
self.points = int(self.TimeMPC/self.dt)
self.v0 = x[0]
self.ClosedLoopDataLTI_MPC = ClosedLoopData(self.points, self.v0)
self.Controller_PathFollowingLTI_MPC = PathFollowingLTI_MPC(self.A, self.B, self.Q, self.R, self.N, self.vt, self.inputConstr)
# simulator.Sim(ClosedLoopDataLTI_MPC, Controller_PathFollowingLTI_MPC)
self.count = 0
def LTIcont(self, x, x_glob, LapTime, SimTime, EndFlag=0):
# ======================================================================================================================
# ====================================== LINEAR REGRESSION ============================================================
# ======================================================================================================================
if EndFlag == 0:
u, new_x, new_x_glob = self.simulator.Sim(x, x_glob, SimTime, self.Controller_PathFollowingLTI_MPC)
self.ClosedLoopDataLTI_MPC.UpdateLoop(self.count, x, x_glob, u, SimTime)
self.count += 1
return new_x, new_x_glob
else:
file_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTI_MPC.obj', 'wb')
pickle.dump(self.ClosedLoopDataLTI_MPC, file_data)
file_data.close()
print("===== MPC terminated")
return new_x, new_x_glob
def LTVinit(self, x, LapTime):
print("Starting TV-MPC")
file_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataPID.obj', 'rb')
ClosedLoopDataPID = pickle.load(file_data)
self.TimeMPC_tv = LapTime
self.points = int(self.TimeMPC_tv/self.dt)
self.v0 = x[0]
self.ClosedLoopDataLTV_MPC = ClosedLoopData(self.points, self.v0)
self.Controller_PathFollowingLTV_MPC = PathFollowingLTV_MPC(self.Q, self.R, self.N, self.vt, self.n, self.d, ClosedLoopDataPID.x, ClosedLoopDataPID.u, self.dt, self.map, self.inputConstr)
# simulator.Sim(ClosedLoopDataLTI_MPC, Controller_PathFollowingLTI_MPC)
self.count = 0
file_data.close()
def LTVcont(self, x, x_glob, LapTime, SimTime, EndFlag=0):
# ======================================================================================================================
# =================================== LOCAL LINEAR REGRESSION =========================================================
# ======================================================================================================================
if EndFlag == 0:
u, new_x, new_x_glob = self.simulator.Sim(x, x_glob, SimTime, self.Controller_PathFollowingLTV_MPC)
self.ClosedLoopDataLTV_MPC.UpdateLoop(self.count, x, x_glob, u, SimTime)
self.count += 1
return new_x, new_x_glob
else:
file_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTV_MPC.obj', 'wb')
pickle.dump(self.ClosedLoopDataLTV_MPC, file_data)
file_data.close()
print("===== TV-MPC terminated")
return new_x, new_x_glob
# ======================================================================================================================
# ============================== LMPC w\ LOCAL LINEAR REGRESSION ======================================================
# ======================================================================================================================
def LMPCinit(self, x, LapTime):
print("Starting LMPC")
self.TimeLMPC = LapTime
self.points = int(self.TimeLMPC/self.dt)
self.v0 = x[0]
self.ClosedLoopLMPC = ClosedLoopData(self.points, self.v0)
self.LMPCOpenLoopData = LMPCprediction(self.N, self.n, self.d, self.TimeLMPC, self.numSS_Points, self.Laps)
self.LMPCSimulator = Simulator(self.Dynamic, map, 1, 1)
self.LMPController = ControllerLMPC(self.numSS_Points, self.numSS_it, self.N, self.Qslack, self.Qlane, self.Q_LMPC,
self.R_LMPC, self.dR_LMPC, self.dt, self.map, self.Laps, self.TimeLMPC, self.LMPC_Solver, self.inputConstr)
PID_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataPID.obj', 'rb')
ClosedLoopDataPID = pickle.load(PID_data)
LTV_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTV_MPC.obj','rb')
ClosedLoopDataLTV_MPC = pickle.load(LTV_data)
LTI_data = open('D:\CARLA\PythonAPI\examples\data\ClosedLoopDataLTI_MPC.obj','rb')
ClosedLoopDataLTI_MPC = pickle.load(LTI_data)
# Run first 4 laps. Remaining laps run by LMPC
self.LMPController.addTrajectory(ClosedLoopDataPID)
self.LMPController.addTrajectory(ClosedLoopDataLTV_MPC)
self.LMPController.addTrajectory(ClosedLoopDataPID)
self.LMPController.addTrajectory(ClosedLoopDataLTI_MPC)
PID_data.close()
LTV_data.close()
LTI_data.close()
self.count = 0
def LMPCcont(self, x, x_glob, LapTime, SimTime, LapTrigger=0, EndFlag=0):
if EndFlag == 0:
u, new_x, new_x_glob = self.LMPCSimulator.Sim(x, x_glob, SimTime, self.LMPController, self.LMPCOpenLoopData)
self.ClosedLoopLMPC.UpdateLoop(self.count, x, x_glob, u, SimTime)
self.count += 1
if LapTrigger == 1:
self.LMPController.addTrajectory(self.ClosedLoopLMPC)
self.LMPCSimulator.reset()
self.count = 0
return new_x, new_x_glob
else:
file_data = open('D:\CARLA\PythonAPI\examples\data\LMPController.obj', 'wb')
pickle.dump(self.ClosedLoopLMPC, file_data)
pickle.dump(self.LMPController, file_data)
pickle.dump(self.LMPCOpenLoopData, file_data)
file_data.close()
return new_x, new_x_glob
print("===== LMPC terminated")
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([
10, 1, 1, 1, 10, 1