Example #1
0
 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        
Example #2
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
Example #3
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,
Example #5
0
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 ============================================================
# ======================================================================================================================
Example #6
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

# <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,
Example #7
0
 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
Example #8
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,