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
ClosedLoopDataLTV_MPC = pickle.load(file_data)
file_data.close()
print("===== TV-MPC terminated")
if plotFlagMPC_tv == 1:
plotTrajectory(map, ClosedLoopDataLTV_MPC.x, ClosedLoopDataLTV_MPC.x_glob,
ClosedLoopDataLTV_MPC.u)
plt.show()
# ======================================================================================================================
# ============================== LMPC w\ LOCAL LINEAR REGRESSION ======================================================
# ======================================================================================================================
raw_input("Finished TV-MPC - Start LMPC?")
print("Starting LMPC")
ClosedLoopLMPC = ClosedLoopData(dt, TimeLMPC, v0)
LMPCOpenLoopData = LMPCprediction(
N, n, d, TimeLMPC, numSS_Points,
Laps) #to store open-loop prediction and safe sets
LMPCSimulator = Simulator(
map, 1,
1) #now this simulator only runs for one lap, with the LMPC flag ON
if runPWAFlag == 1:
LMPController = PWAControllerLMPC(10, numSS_Points, numSS_it, N, Qslack,
Q_LMPC, R_LMPC, dR_LMPC, n, d, shift, dt,
map, Laps, TimeLMPC, LMPC_Solver)
else:
LMPController = ControllerLMPC(numSS_Points, numSS_it, N, Qslack, Q_LMPC,
R_LMPC, dR_LMPC, n, d, shift, dt, map, Laps,
TimeLMPC, LMPC_Solver)
onlyLMPController = ControllerLMPC(numSS_Points, numSS_it, N, Qslack,
Q_LMPC, R_LMPC, dR_LMPC, n, d, shift,
simulator.Sim(ClosedLoopDataLTV_MPC, Controller_PathFollowingLTV_MPC)
file_data = open(sys.path[0] + '/data/ClosedLoopDataLTV_MPC1.obj', 'w')
pickle.dump(ClosedLoopDataLTV_MPC, file_data)
file_data.close()
else:
file_data = open(sys.path[0] + '/data/ClosedLoopDataLTV_MPC1.obj', 'r')
ClosedLoopDataLTV_MPC = pickle.load(file_data)
file_data.close()
print "===== TV-MPC terminated"
# ======================================================================================================================
# ===========================================原 LMPC 控制 ======================================================
# ======================================================================================================================
print "Starting LMPC"
ClosedLoopLMPC = ClosedLoopDataLMPC(dt, TimeLMPC, v0, Laps)
LMPCOpenLoopData = LMPCprediction(N, n, d, TimeLMPC, numSS_Points,
Laps) #生成所需大小的初始零矩阵
LMPCSimulator = Simulator(map, 1, 1)
LMPController = ControllerLMPC(numSS_Points, numSS_it, N, Qslack, Qlane,
Q_LMPC, R_LMPC, dR_LMPC, dt, map, Laps,
TimeLMPC, LMPC_Solver, inputConstr)
LMPController.addTrajectory(ClosedLoopDataPID)
LMPController.addTrajectory(ClosedLoopDataLTV_MPC)
x0 = np.zeros((1, n))
x0_glob = np.zeros((1, n))
x0[0, :] = ClosedLoopLMPC.x[0, :]
x0_glob[0, :] = ClosedLoopLMPC.x_glob[0, :]
if RunLMPC == 1:
for it in range(2, Laps):
file_data = open('data/ClosedLoopDataLTI_MPC.obj', 'rb')
ClosedLoopDataLTI_MPC = pickle.load(file_data)
file_data.close()
if plotFlagMPC == 1:
plotTrajectory(map, ClosedLoopDataLTI_MPC.x, ClosedLoopDataLTI_MPC.x_glob,
ClosedLoopDataLTI_MPC.u)
plt.show()
# ======================================================================================================================
# ============================== LMPC w\ LTI REGRESSION-===============================================================
# ======================================================================================================================
raw_input("Finished LTI Tracking MPC - Start LTI-LMPC?")
ClosedLoopLMPC = ClosedLoopData(dt, TimeLMPC, v0)
LMPCOpenLoopData = LMPCprediction(
N, n, d, TimeLMPC, numSS_Points,
Laps) #to store open-loop prediction and safe sets
LMPCSimulator = Simulator(
map, 1,
1) #now this simulator only runs for one lap, with the LMPC flag ON
if runPWAFlag == 1:
LMPController = PWAControllerLMPC(10, numSS_Points, numSS_it, N, Qslack,
Q_LMPC, R_LMPC, dR_LMPC, n, d, shift, dt,
map, Laps, TimeLMPC, LMPC_Solver)
else:
LMPController = ControllerLTI_LMPC(numSS_Points, numSS_it, N, Qslack,
Q_LMPC, R_LMPC, dR_LMPC, n, d, shift,
dt, map, Laps, TimeLMPC, LMPC_Solver)
# add previously completed trajectories to Safe Set: