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 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 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()
]) # Cost on the slack variable for the terminal constraint
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]
R_LMPC = 1 * np.diag([1.0, 1.0]) # Input cost u = [delta, a]
dR_LMPC = 5 * np.array([1.0, 1.0]) # Input rate cost u
# Initialize LMPC simulator
LMPCSimulator = Simulator(
map, 1, 1) #flags indicate one lap, and using the LMPC controller
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
print("Starting PID")
if RunPID == 1:
ClosedLoopDataPID = ClosedLoopData(dt, Time,
v0) #form matrices for experiment data
PIDController = PID(vt) #sets the reference velocity and some timers?
simulator.Sim(
ClosedLoopDataPID,
PIDController) #simulates the PID controller for Time timesteps
file_data = open('data/ClosedLoopDataPID.obj', 'wb')
pickle.dump(ClosedLoopDataPID, file_data)
file_data.close()
else:
file_data = open('data/ClosedLoopDataPID.obj', 'rb')
ClosedLoopDataPID = pickle.load(file_data)
file_data.close()
print("===== PID terminated")
if plotFlag == 1:
plotTrajectory(map, ClosedLoopDataPID.x, ClosedLoopDataPID.x_glob,
R_LMPC = 0 * np.diag([1.0, 1.0]) # Input cost u = [delta, a]
dR_LMPC = 10 * np.array([1.0, 10.0]) # Input rate cost u
#输入约束的边界值
inputConstr = np.array([
[0.5, 0.5], # Min Steering and Max Steering
[1.0, 1.0]
]) # Min Acceleration and Max Acceleration
# Initialize LMPC simulator
LMPCSimulator = Simulator(map, 1, 1)
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
print "Starting PID"
if RunPID == 1:
ClosedLoopDataPID = ClosedLoopData(dt, Time, v0)
PIDController = PID(vt)
simulator.Sim(ClosedLoopDataPID, PIDController)
file_data = open(sys.path[0] + '/data/ClosedLoopDataPID1.obj', 'wb')
pickle.dump(ClosedLoopDataPID, file_data)
file_data.close()
else:
file_data = open(sys.path[0] + '/data/ClosedLoopDataPID1.obj', 'rb')
ClosedLoopDataPID = pickle.load(file_data)
file_data.close()
print "===== PID terminated"
# ======================================================================================================================
# ====================================== LINEAR REGRESSION ============================================================
# ======================================================================================================================
]) # State cost x = [vx, vy, wz, epsi, s, ey]
R_LMPC = 1 * np.diag([1.0, 1.0]) # Input cost u = [delta, a]
dR_LMPC = 5 * np.array([1.0, 1.0]) # Input rate cost u
# Initialize LMPC simulator
LMPCSimulator = Simulator(
map, 1, 1) #flags indicate one lap, and using the LMPC controller
# <codecell>
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
print("Starting PID")
if RunPID == 1:
ClosedLoopDataPID = ClosedLoopData(dt, Time,
v0) #form matrices for experiment data
PIDController = PID(vt) #sets the reference velocity and some timers?
simulator.Sim(
ClosedLoopDataPID,
PIDController) #simulates the PID controller for Time timesteps
file_data = open('data/ClosedLoopDataPID.obj', 'wb')
pickle.dump(ClosedLoopDataPID, file_data)
file_data.close()
else:
file_data = open('data/ClosedLoopDataPID.obj', 'rb')
ClosedLoopDataPID = pickle.load(file_data)
file_data.close()
if plotFlag == 1:
plotTrajectory(map, ClosedLoopDataPID.x, ClosedLoopDataPID.x_glob,
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
]) # Cost on the slack variable for the terminal constraint
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]
R_LMPC = 1 * np.diag([1.0, 1.0]) # Input cost u = [delta, a]
dR_LMPC = 5 * np.array([1.0, 1.0]) # Input rate cost u
# Initialize LMPC simulator
LMPCSimulator = Simulator(
map, 1, 1) #flags indicate one lap, and using the LMPC controller
# ======================================================================================================================
# ======================================= PID path following ===========================================================
# ======================================================================================================================
print("Starting PID")
if RunPID == 1:
ClosedLoopDataPID = ClosedLoopData(dt, Time,
v0) #form matrices for experiment data
PIDController = PID(vt) #sets the reference velocity and some timers?
simulator.Sim(
ClosedLoopDataPID,
PIDController) #simulates the PID controller for Time timesteps
file_data = open('data/ClosedLoopDataPID.obj', 'wb')
pickle.dump(ClosedLoopDataPID, file_data)
file_data.close()
else:
file_data = open('data/ClosedLoopDataPID.obj', 'rb')
ClosedLoopDataPID = pickle.load(file_data)
file_data.close()
print("===== PID terminated")
if plotFlag == 1:
plotTrajectory(map, ClosedLoopDataPID.x, ClosedLoopDataPID.x_glob,