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
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,
ClosedLoopDataPID.u)
plt.show()
# ======================================================================================================================
# ====================================== LINEAR REGRESSION ============================================================
# ======================================================================================================================
raw_input("Finished PID - Start TI-MPC?")
print("Starting TI-MPC")
lamb = 0.0000001
#fit linear dynamics to the closed loop data: x2 = A*x1 + b*u1; lamb is weight on frob norm of W
A, B, Error = Regression(ClosedLoopDataPID.x, ClosedLoopDataPID.u, lamb)
if RunMPC == 1:
ClosedLoopDataLTI_MPC = ClosedLoopData(
dt, TimeMPC, v0) #form (empty) matrices for experiment data
Controller_PathFollowingLTI_MPC = PathFollowingLTI_MPC(A, B, Q, R, N, vt)
simulator.Sim(ClosedLoopDataLTI_MPC, Controller_PathFollowingLTI_MPC)
#file_data = open(sys.path[0]+'/data/ClosedLoopDataLTI_MPC.obj', 'wb')
file_data = open('data/ClosedLoopDataLTI_MPC.obj', 'wb')
pickle.dump(ClosedLoopDataLTI_MPC, file_data)
file_data.close()
else:
#file_data = open(sys.path[0]+'/data/ClosedLoopDataLTI_MPC.obj', 'rb')
file_data = open('data/ClosedLoopDataLTI_MPC.obj', 'rb')
ClosedLoopDataLTI_MPC = pickle.load(file_data)
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()