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()
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, 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)
LMPController = pickle.load(file_data) LMPCOpenLoopData = pickle.load(file_data) file_data.close() print "===== LMPC terminated" # ====================================================================================================================== # ========================================= PLOT TRACK ================================================================= # ====================================================================================================================== for i in range(0, LMPController.it): print "Lap time at iteration ", i, " is ", LMPController.Qfun[0, i] * dt, "s" print "===== Start Plotting" if plotFlag == 1: plotTrajectory(map, ClosedLoopDataPID.x, ClosedLoopDataPID.x_glob, ClosedLoopDataPID.u) if plotFlagMPC == 1: plotTrajectory(map, ClosedLoopDataLTI_MPC.x, ClosedLoopDataLTI_MPC.x_glob, ClosedLoopDataLTI_MPC.u) if plotFlagMPC_tv == 1: plotTrajectory(map, ClosedLoopDataLTV_MPC.x, ClosedLoopDataLTV_MPC.x_glob, ClosedLoopDataLTV_MPC.u) if plotFlagLMPC == 1: plotClosedLoopLMPC(LMPController, map) if animation_xyFlag == 1: #animation_xy(map, LMPCOpenLoopData, LMPController, 3) saveGif_xyResults(map, LMPCOpenLoopData, LMPController, 40)