├── .gitignore
├── README.md
└── src
    ├── ClosedLoop.gif
    ├── ClosedLoopStates.gif
    ├── ClosedLoop_multiLap.gif
    ├── fnc
        ├── Utilities.py
        ├── controller
        │   ├── PredictiveControllers.py
        │   └── PredictiveModel.py
        ├── plot.py
        └── simulator
        │   ├── SysModel.py
        │   └── Track.py
    ├── initControllerParameters.py
    └── main.py


/.gitignore:
--------------------------------------------------------------------------------
1 | *.xml
2 | *.pyc
3 | *.obj
4 | **/*.obj
5 | .DS_Store


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Learning Model Predictive Control (LMPC) for autonomous racing
 2 | 
 3 | The Learning Model Predictive Control (LMPC) is a data-driven control framework developed at UCB in the MPC lab. In this example, we implemented the LMPC for the autonomous racing problem. The controller drives several laps on race track and it learns from experience how to drive faster.
 4 | 
 5 | 
 6 | <p align="center">
 7 | <img src="https://github.com/urosolia/RacingLMPC/blob/master/src/ClosedLoop_multiLap.gif" width="500" />
 8 | </p>
 9 | 
10 | In the above animation we see the vehicle's closed-loop trajectory (in black) for laps 5, 30, 31 and 32. At each time instant the LMPC leverages forecast to plan the vehicle trajectory (in red) few seconds into the future. This trajectory is planned to minimize the lap time, but it is constrained to land into the safe set (in green). This safe set is the domain of the approximation to the value function and it is updated after each lap using historical data.
11 | 
12 | ### Prerequisites
13 | 
14 | The packeges needed for running the code can be installed using pip
15 | 
16 | ```
17 | pip install cvxopt
18 | pip install osqp
19 | pip install pathos
20 | ```
21 | 
22 | ## Description
23 | 
24 | ### The Plant
25 | The vehicle is modelled using the dynamics signle track bicycle model and the tire forces are modelled using the Pacejka formula.
26 | 
27 | ### The Path Following
28 | 1) Lap 1: a PID path following controller is used to drive the vehicle around the track.
29 | 2) Lap 2: the data from lap 1 are used to estimate a LTI model used to design a MPC for path following
30 | 3) Lap 3: the data from lap 1 are used to estimate a LTV model used to design a MPC for path following
31 | 
32 | 
33 | ## References
34 | 
35 | This code is based on the following:
36 | 
37 | * Ugo Rosolia and Francesco Borrelli. "Learning Model Predictive Control for Iterative Tasks. A Data-Driven Control Framework." In IEEE Transactions on Automatic Control (2017). [PDF](https://ieeexplore.ieee.org/document/8039204/)
38 | * Ugo Rosolia and Francesco Borrelli. "Learning how to autonomously race a car: a predictive control approach." IEEE Transactions on Control Systems Technology (2019) [PDF](https://ieeexplore.ieee.org/abstract/document/8896988).
39 | * Ugo Rosolia and Francesco Borrelli. "Learning Model Predictive Control for Iterative Tasks: A Computationally Efficient Approach for Linear System." IFAC-PapersOnLine 50.1 (2017). [PDF](https://arxiv.org/pdf/1702.07064.pdf)
40 | 


--------------------------------------------------------------------------------
/src/ClosedLoop.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/urosolia/RacingLMPC/b49c95e7795ef6f343efacedb6b90a69c7855b32/src/ClosedLoop.gif


--------------------------------------------------------------------------------
/src/ClosedLoopStates.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/urosolia/RacingLMPC/b49c95e7795ef6f343efacedb6b90a69c7855b32/src/ClosedLoopStates.gif


--------------------------------------------------------------------------------
/src/ClosedLoop_multiLap.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/urosolia/RacingLMPC/b49c95e7795ef6f343efacedb6b90a69c7855b32/src/ClosedLoop_multiLap.gif


--------------------------------------------------------------------------------
/src/fnc/Utilities.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import pdb
 3 | import datetime
 4 | 
 5 | def Regression(x, u, lamb):
 6 |     """Estimates linear system dynamics
 7 |     x, u: date used in the regression
 8 |     lamb: regularization coefficient
 9 |     """
10 | 
11 |     # Want to solve W^* = argmin sum_i ||W^T z_i - y_i ||_2^2 + lamb ||W||_F,
12 |     # with z_i = [x_i u_i] and W \in R^{n + d} x n
13 |     Y = x[2:x.shape[0], :]
14 |     X = np.hstack((x[1:(x.shape[0] - 1), :], u[1:(x.shape[0] - 1), :]))
15 | 
16 |     Q = np.linalg.inv(np.dot(X.T, X) + lamb * np.eye(X.shape[1]))
17 |     b = np.dot(X.T, Y)
18 |     W = np.dot(Q, b)
19 | 
20 |     A = W.T[:, 0:6]
21 |     B = W.T[:, 6:8]
22 | 
23 |     ErrorMatrix = np.dot(X, W) - Y
24 |     ErrorMax = np.max(ErrorMatrix, axis=0)
25 |     ErrorMin = np.min(ErrorMatrix, axis=0)
26 |     Error = np.vstack((ErrorMax, ErrorMin))
27 | 
28 |     return A, B, Error
29 | 
30 | 
31 | def wrap(angle):
32 |     if angle < -np.pi:
33 |         w_angle = 2 * np.pi + angle
34 |     elif angle > np.pi:
35 |         w_angle = angle - 2 * np.pi
36 |     else:
37 |         w_angle = angle
38 | 
39 |     return w_angle
40 | 
41 | 
42 | class PID:
43 |     """Create the PID controller used for path following at constant speed
44 |     Attributes:
45 |         solve: given x0 computes the control action
46 |     """
47 |     def __init__(self, vt):
48 |         """Initialization
49 |         Arguments:
50 |             vt: target velocity
51 |         """
52 |         self.vt = vt
53 |         self.uPred = np.zeros([1,2])
54 | 
55 |         startTimer = datetime.datetime.now()
56 |         endTimer = datetime.datetime.now(); deltaTimer = endTimer - startTimer
57 |         self.solverTime = deltaTimer
58 |         self.linearizationTime = deltaTimer
59 |         self.feasible = 1
60 | 
61 |     def solve(self, x0):
62 |         """Computes control action
63 |         Arguments:
64 |             x0: current state position
65 |         """
66 |         vt = self.vt
67 |         self.uPred[0, 0] = - 0.6 * x0[5] - 0.9 * x0[3] + np.max([-0.9, np.min([np.random.randn() * 0.25, 0.9])])
68 |         self.uPred[0, 1] = 1.5 * (vt - x0[0]) + np.max([-0.2, np.min([np.random.randn() * 0.10, 0.2])])


--------------------------------------------------------------------------------
/src/fnc/controller/PredictiveControllers.py:
--------------------------------------------------------------------------------
  1 | import pdb
  2 | import numpy as np
  3 | from cvxopt import spmatrix, matrix, solvers
  4 | from numpy import linalg as la
  5 | from scipy import linalg
  6 | from scipy import sparse
  7 | from cvxopt.solvers import qp
  8 | import datetime
  9 | from numpy import hstack, inf, ones
 10 | from scipy.sparse import vstack
 11 | from osqp import OSQP
 12 | from dataclasses import dataclass, field
 13 | 
 14 | solvers.options['show_progress'] = False
 15 | 
 16 | @dataclass
 17 | class PythonMsg:
 18 |     def __setattr__(self,key,value):
 19 |         if not hasattr(self,key):
 20 |             raise TypeError ('Cannot add new field "%s" to frozen class %s' %(key,self))
 21 |         else:
 22 |             object.__setattr__(self,key,value)
 23 | 
 24 | @dataclass
 25 | class MPCParams(PythonMsg):
 26 |     n: int = field(default=None) # dimension state space
 27 |     d: int = field(default=None) # dimension input space
 28 |     N: int = field(default=None) # horizon length
 29 | 
 30 |     A: np.array = field(default=None) # prediction matrices. Single matrix for LTI and list for LTV
 31 |     B: np.array = field(default=None) # prediction matrices. Single matrix for LTI and list for LTV
 32 | 
 33 |     Q: np.array = field(default=np.array((n, n))) # quadratic state cost
 34 |     R: np.array = field(default=None) # quadratic input cost
 35 |     Qf: np.array = field(default=None) # quadratic state cost final
 36 |     dR: np.array = field(default=None) # Quadratic rate cost
 37 |     
 38 |     Qslack: float = field(default=None) # it has to be a vector. Qslack = [linearSlackCost, quadraticSlackCost]
 39 |     Fx: np.array = field(default=None) # State constraint Fx * x <= bx
 40 |     bx: np.array = field(default=None)
 41 |     Fu: np.array = field(default=None) # State constraint Fu * u <= bu
 42 |     bu: np.array = field(default=None)
 43 |     xRef: np.array = field(default=None)
 44 | 
 45 |     slacks: bool = field(default=True)
 46 |     timeVarying: bool = field(default=False)
 47 | 
 48 |     def __post_init__(self):
 49 |         if self.Qf is None: self.Qf = np.zeros((self.n, self.n))
 50 |         if self.dR is None: self.dR = np.zeros(self.d)
 51 |         if self.xRef is None: self.xRef = np.zeros(self.n)
 52 | 
 53 | ############################################################################################
 54 | ####################################### MPC CLASS ##########################################
 55 | ############################################################################################
 56 | class MPC():
 57 |     """Model Predicitve Controller class
 58 |     Methods (needed by user):
 59 |         solve: given system's state xt compute control action at
 60 |     Arguments:
 61 |         mpcParameters: model paramters
 62 |     """
 63 |     def __init__(self,  mpcParameters, predictiveModel=[]):
 64 |         """Initialization
 65 |         Arguments:
 66 |             mpcParameters: struct containing MPC parameters
 67 |         """
 68 |         self.N      = mpcParameters.N
 69 |         self.Qslack = mpcParameters.Qslack
 70 |         self.Q      = mpcParameters.Q
 71 |         self.Qf     = mpcParameters.Qf
 72 |         self.R      = mpcParameters.R
 73 |         self.dR     = mpcParameters.dR
 74 |         self.n      = mpcParameters.n
 75 |         self.d      = mpcParameters.d
 76 |         self.A      = mpcParameters.A
 77 |         self.B      = mpcParameters.B
 78 |         self.Fx     = mpcParameters.Fx
 79 |         self.Fu     = mpcParameters.Fu
 80 |         self.bx     = mpcParameters.bx
 81 |         self.bu     = mpcParameters.bu
 82 |         self.xRef   = mpcParameters.xRef
 83 | 
 84 |         self.slacks          = mpcParameters.slacks
 85 |         self.timeVarying     = mpcParameters.timeVarying
 86 |         self.predictiveModel = predictiveModel
 87 | 
 88 |         if self.timeVarying == True:
 89 |             self.xLin = self.predictiveModel.xStored[-1][0:self.N+1,:]
 90 |             self.uLin = self.predictiveModel.uStored[-1][0:self.N,:]
 91 |             self.computeLTVdynamics()
 92 |         
 93 |         self.OldInput = np.zeros((1,2)) # TO DO fix size
 94 | 
 95 |         # Build matrices for inequality constraints
 96 |         self.buildIneqConstr()
 97 |         self.buildCost()
 98 |         self.buildEqConstr()
 99 | 
100 |         self.xPred = []
101 | 
102 |         # initialize time
103 |         startTimer = datetime.datetime.now()
104 |         endTimer = datetime.datetime.now(); deltaTimer = endTimer - startTimer
105 |         self.solverTime = deltaTimer
106 |         self.linearizationTime = deltaTimer
107 |         self.timeStep = 0
108 | 
109 | 
110 |     def solve(self, x0):
111 |         """Computes control action
112 |         Arguments:
113 |             x0: current state
114 |         """
115 |         # If LTV active --> identify system model
116 |         if self.timeVarying == True:
117 |             self.computeLTVdynamics()
118 |             self.buildCost()
119 |             self.buildEqConstr()
120 |         
121 |         self.addTerminalComponents(x0)
122 |         # Solve QP
123 |         startTimer = datetime.datetime.now()
124 |         self.osqp_solve_qp(self.H_FTOCP, self.q_FTOCP, self.F_FTOCP, self.b_FTOCP, self.G_FTOCP, np.add(np.dot(self.E_FTOCP,x0),self.L_FTOCP))
125 |         self.unpackSolution()
126 |         endTimer = datetime.datetime.now(); deltaTimer = endTimer - startTimer
127 |         self.solverTime = deltaTimer
128 | 
129 |         # If LTV active --> compute state-input linearization trajectory
130 |         self.feasibleStateInput()
131 |         if self.timeVarying == True:
132 |             self.xLin = np.vstack((self.xPred[1:, :], self.zt))
133 |             self.uLin = np.vstack((self.uPred[1:, :], self.zt_u))
134 | 
135 |         # update applied input
136 |         self.OldInput = self.uPred[0,:]
137 |         self.timeStep += 1
138 | 
139 | 
140 |     def computeLTVdynamics(self):
141 |         # Estimate system dynamics
142 |         self.A = []; self.B = []; self.C =[]
143 |         for i in range(0, self.N):
144 |             Ai, Bi, Ci = self.predictiveModel.regressionAndLinearization(self.xLin[i], self.uLin[i])
145 |             self.A.append(Ai); self.B.append(Bi); self.C.append(Ci)
146 | 
147 |     def addTerminalComponents(self, x0):
148 |         # TO DO: ....
149 |         self.H_FTOCP = sparse.csc_matrix(self.H)
150 |         self.q_FTOCP = self.q
151 |         self.F_FTOCP = sparse.csc_matrix(self.F)
152 |         self.b_FTOCP = self.b
153 |         self.G_FTOCP = sparse.csc_matrix(self.G)
154 |         self.E_FTOCP = self.E
155 |         self.L_FTOCP = self.L
156 | 
157 |     def feasibleStateInput(self):
158 |         self.zt   = self.xPred[-1,:]
159 |         self.zt_u = self.uPred[-1,:]
160 | 
161 |     def unpackSolution(self):
162 |         # Extract predicted state and predicted input trajectories
163 |         self.xPred = np.squeeze(np.transpose(np.reshape((self.Solution[np.arange(self.n*(self.N+1))]),(self.N+1,self.n)))).T
164 |         self.uPred = np.squeeze(np.transpose(np.reshape((self.Solution[self.n*(self.N+1)+np.arange(self.d*self.N)]),(self.N, self.d)))).T
165 |         
166 |     def buildIneqConstr(self):
167 |         # The inequality constraint is Fz<=b
168 |         # Let's start by computing the submatrix of F relates with the state
169 |         rep_a = [self.Fx] * (self.N)
170 |         Mat = linalg.block_diag(*rep_a)
171 |         NoTerminalConstr = np.zeros((np.shape(Mat)[0], self.n))  # The last state is unconstrained. There is a specific function add the terminal constraints (so that more complicated terminal constrains can be handled)
172 |         Fxtot = np.hstack((Mat, NoTerminalConstr))
173 |         bxtot = np.tile(np.squeeze(self.bx), self.N)
174 | 
175 |         # Let's start by computing the submatrix of F relates with the input
176 |         rep_b = [self.Fu] * (self.N)
177 |         Futot = linalg.block_diag(*rep_b)
178 |         butot = np.tile(np.squeeze(self.bu), self.N)
179 | 
180 |         # Let's stack all together
181 |         F_hard = linalg.block_diag(Fxtot, Futot)
182 | 
183 |         # Add slack if need
184 |         if self.slacks == True:
185 |             nc_x = self.Fx.shape[0] # add slack only for state constraints
186 |             # Fist add add slack to existing constraints
187 |             addSlack = np.zeros((F_hard.shape[0], nc_x*self.N))
188 |             addSlack[0:nc_x*(self.N), 0:nc_x*(self.N)] = -np.eye(nc_x*(self.N))
189 |             # Now constraint slacks >= 0
190 |             I = - np.eye(nc_x*self.N); Zeros = np.zeros((nc_x*self.N, F_hard.shape[1]))
191 |             Positivity = np.hstack((Zeros, I))
192 | 
193 |             # Let's stack all together
194 |             self.F = np.vstack(( np.hstack((F_hard, addSlack)) , Positivity))
195 |             self.b = np.hstack((bxtot, butot, np.zeros(nc_x*self.N)))
196 |         else:
197 |             self.F = F_hard
198 |             self.b = np.hstack((bxtot, butot))
199 | 
200 |     def buildEqConstr(self):
201 |         # Buil matrices for optimization (Convention from Chapter 15.2 Borrelli, Bemporad and Morari MPC book)
202 |         # The equality constraint is: G*z = E * x(t) + L
203 |         Gx = np.eye(self.n * (self.N + 1))
204 |         Gu = np.zeros((self.n * (self.N + 1), self.d * (self.N)))
205 | 
206 |         E = np.zeros((self.n * (self.N + 1), self.n))
207 |         E[np.arange(self.n)] = np.eye(self.n)
208 | 
209 |         L = np.zeros(self.n * (self.N + 1))
210 | 
211 |         for i in range(0, self.N):
212 |             if self.timeVarying == True:
213 |                 Gx[(self.n + i*self.n):(self.n + i*self.n + self.n), (i*self.n):(i*self.n + self.n)] = -self.A[i]
214 |                 Gu[(self.n + i*self.n):(self.n + i*self.n + self.n), (i*self.d):(i*self.d + self.d)] = -self.B[i]
215 |                 L[(self.n + i*self.n):(self.n + i*self.n + self.n)]                                  =  self.C[i]
216 |             else:
217 |                 Gx[(self.n + i*self.n):(self.n + i*self.n + self.n), (i*self.n):(i*self.n + self.n)] = -self.A
218 |                 Gu[(self.n + i*self.n):(self.n + i*self.n + self.n), (i*self.d):(i*self.d + self.d)] = -self.B
219 | 
220 |         if self.slacks == True:
221 |             self.G = np.hstack( (Gx, Gu, np.zeros( ( Gx.shape[0], self.Fx.shape[0]*self.N) ) ) ) 
222 |         else:
223 |             self.G = np.hstack((Gx, Gu))
224 |     
225 |         self.E = E
226 |         self.L = L
227 | 
228 |     def buildCost(self):
229 |         # The cost is: (1/2) * z' H z + q' z
230 |         listQ = [self.Q] * (self.N)
231 |         Hx = linalg.block_diag(*listQ)
232 | 
233 |         listTotR = [self.R + 2 * np.diag(self.dR)] * (self.N) # Need to add dR for the derivative input cost
234 |         Hu = linalg.block_diag(*listTotR)
235 |         # Need to condider that the last input appears just once in the difference
236 |         for i in range(0, self.d):
237 |             Hu[ i - self.d, i - self.d] = Hu[ i - self.d, i - self.d] - self.dR[i]
238 | 
239 |         # Derivative Input Cost
240 |         OffDiaf = -np.tile(self.dR, self.N-1)
241 |         np.fill_diagonal(Hu[self.d:], OffDiaf)
242 |         np.fill_diagonal(Hu[:, self.d:], OffDiaf)
243 |         
244 |         # Cost linear term for state and input
245 |         q = - 2 * np.dot(np.append(np.tile(self.xRef, self.N + 1), np.zeros(self.R.shape[0] * self.N)), linalg.block_diag(Hx, self.Qf, Hu))
246 |         # Derivative Input (need to consider input at previous time step)
247 |         q[self.n*(self.N+1):self.n*(self.N+1)+self.d] = -2 * np.dot( self.OldInput, np.diag(self.dR) )
248 |         if self.slacks == True: 
249 |             quadSlack = self.Qslack[0] * np.eye(self.Fx.shape[0]*self.N)
250 |             linSlack  = self.Qslack[1] * np.ones(self.Fx.shape[0]*self.N )
251 |             self.H = linalg.block_diag(Hx, self.Qf, Hu, quadSlack)
252 |             self.q = np.append(q, linSlack)
253 |         else: 
254 |             self.H = linalg.block_diag(Hx, self.Qf, Hu)
255 |             self.q = q 
256 |  
257 |         self.H = 2 * self.H  #  Need to multiply by two because CVX considers 1/2 in front of quadratic cost
258 | 
259 |     def osqp_solve_qp(self, P, q, G= None, h=None, A=None, b=None, initvals=None):
260 |         """ 
261 |         Solve a Quadratic Program defined as:
262 |         minimize
263 |             (1/2) * x.T * P * x + q.T * x
264 |         subject to
265 |             G * x <= h
266 |             A * x == b
267 |         using OSQP <https://github.com/oxfordcontrol/osqp>.
268 |         """  
269 |         self.osqp = OSQP()
270 |         qp_A = vstack([G, A]).tocsc()
271 |         l = -inf * ones(len(h))
272 |         qp_l = hstack([l, b])
273 |         qp_u = hstack([h, b])
274 | 
275 |         self.osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
276 |         if initvals is not None:
277 |             self.osqp.warm_start(x=initvals)
278 |         res = self.osqp.solve()
279 |         if res.info.status_val == 1:
280 |             self.feasible = 1
281 |         else:
282 |             self.feasible = 0
283 |         self.Solution = res.x
284 | 
285 | ############## Below LMPC class which is a child of the MPC super class
286 | class LMPC(MPC):
287 |     """Create the LMPC
288 |     Methods (needed by user):
289 |         solve: given x0 computes the control action
290 |         addTrajectory: given a ClosedLoopData object adds the trajectory to SS, Qfun, uSS and updates the iteration index
291 |         addPoint: this function allows to add the closed loop data at iteration j to the SS of iteration (j-1)
292 |     """
293 |     def __init__(self, numSS_Points, numSS_it, QterminalSlack, mpcPrameters, predictiveModel, dt = 0.1):
294 |         """Initialization
295 |         Arguments:
296 |             numSS_Points: number of points selected from the previous trajectories to build SS
297 |             numSS_it: number of previois trajectories selected to build SS
298 |             N: horizon length
299 |             Q,R: weight to define cost function h(x,u) = ||x||_Q + ||u||_R
300 |             dR: weight to define the input rate cost h(x,u) = ||x_{k+1}-x_k||_dR
301 |             n,d: state and input dimensiton
302 |             shift: given the closest point x_t^j to x(t) the controller start selecting the point for SS from x_{t+shift}^j
303 |             map: map
304 |             Laps: maximum number of laps the controller can run (used to avoid dynamic allocation)
305 |             TimeLMPC: maximum time [s] that an lap can last (used to avoid dynamic allocation)
306 |             Solver: solver used in the reformulation of the LMPC as QP
307 |         """
308 |         super().__init__(mpcPrameters, predictiveModel)
309 |         self.numSS_Points = numSS_Points
310 |         self.numSS_it     = numSS_it
311 |         self.QterminalSlack = QterminalSlack
312 | 
313 |         self.OldInput = np.zeros((1,2))
314 |         self.xPred    = []
315 | 
316 |         # Initialize the following quantities to avoid dynamic allocation
317 |         self.LapTime = []        # Time at which each j-th iteration is completed
318 |         self.SS         = []    # Sampled Safe SS
319 |         self.uSS        = []    # Input associated with the points in SS
320 |         self.Qfun       = []       # Qfun: cost-to-go from each point in SS
321 |         self.SS_glob    = []   # SS in global (X-Y) used for plotting
322 | 
323 |         self.xStoredPredTraj     = []
324 |         self.xStoredPredTraj_it  = []
325 |         self.uStoredPredTraj     = []
326 |         self.uStoredPredTraj_it  = []
327 |         self.SSStoredPredTraj    = []
328 |         self.SSStoredPredTraj_it = []
329 | 
330 |         self.zt = np.array([0.0, 0.0, 0.0, 0.0, 10.0, 0.0])
331 | 
332 |         # Initialize the controller iteration
333 |         self.it      = 0
334 | 
335 |         # Build matrices for inequality constraints
336 |         self.buildIneqConstr()
337 |         self.buildCost()
338 |         self.addSafeSetIneqConstr()
339 | 
340 |     def addSafeSetIneqConstr(self):
341 |         # Add positiviti constraints for lambda_{SafeSet}. Note that no constraint is enforced on slack_{SafeSet} ---> add np.hstack(-np.eye(self.numSS_Points), np.zeros(self.n)) 
342 |         self.F_FTOCP = sparse.csc_matrix( linalg.block_diag( self.F, np.hstack((-np.eye(self.numSS_Points), np.zeros((self.numSS_Points, self.n)))) ) )
343 |         self.b_FTOCP = np.append(self.b, np.zeros(self.numSS_Points))
344 |     
345 |     def addSafeSetEqConstr(self):
346 |         # Add constrains for x, u, slack
347 |         xTermCons = np.zeros((self.n, self.G.shape[1]))
348 |         xTermCons[:, self.N * self.n:(self.N + 1) * self.n] = np.eye(self.n)
349 |         G_x_u_slack = np.vstack((self.G, xTermCons))
350 |         # Constraint for lambda_{SaFeSet, slack_{safeset}} to enforce safe set
351 |         G_lambda_slackSafeSet = np.vstack( (np.zeros((self.G.shape[0], self.SS_PointSelectedTot.shape[1] + self.n)), np.hstack((-self.SS_PointSelectedTot, np.eye(self.n)))) )
352 |         # Constraints on lambda = 1
353 |         G_lambda = np.append(np.append(np.zeros(self.G.shape[1]), np.ones(self.SS_PointSelectedTot.shape[1])), np.zeros(self.n))
354 |         # Put all together
355 |         self.G_FTOCP = sparse.csc_matrix(np.vstack((np.hstack((G_x_u_slack, G_lambda_slackSafeSet)), G_lambda)))
356 |         self.E_FTOCP = np.vstack((self.E, np.zeros((self.n+1,self.n)))) # adding n for terminal constraint and 1 for lambda = 1
357 |         self.L_FTOCP = np.append(np.append(self.L, np.zeros(self.n)), 1)
358 | 
359 |     def addSafeSetCost(self):
360 |         # need to multiply the quadratic term as cost is (1/2) z'*Q*z
361 |         self.H_FTOCP = sparse.csc_matrix(linalg.block_diag(self.H, np.zeros((self.SS_PointSelectedTot.shape[1], self.SS_PointSelectedTot.shape[1])), 2*self.QterminalSlack) )
362 |         self.q_FTOCP = np.append(np.append(self.q, self.Qfun_SelectedTot), np.zeros(self.n))
363 |    
364 |     def unpackSolution(self):
365 |         stateIdx = self.n*(self.N+1)
366 |         inputIdx = stateIdx + self.d*self.N
367 |         slackIdx = inputIdx + self.Fx.shape[0]*self.N
368 |         lambdIdx = slackIdx + self.SS_PointSelectedTot.shape[1]
369 |         sTermIdx = lambdIdx + self.n
370 | 
371 |         self.xPred = np.squeeze(np.transpose(np.reshape((self.Solution[np.arange(self.n*(self.N+1))]),(self.N+1,self.n)))).T
372 |         self.uPred = np.squeeze(np.transpose(np.reshape((self.Solution[self.n*(self.N+1)+np.arange(self.d*self.N)]),(self.N, self.d)))).T
373 |         self.slack = self.Solution[inputIdx:slackIdx]
374 |         self.lambd = self.Solution[slackIdx:lambdIdx]
375 |         self.slackTerminal = self.Solution[lambdIdx:]
376 | 
377 |         self.xStoredPredTraj_it.append(self.xPred)
378 |         self.uStoredPredTraj_it.append(self.uPred)
379 |         self.SSStoredPredTraj_it.append(self.SS_PointSelectedTot.T)
380 |         
381 | 
382 |     def feasibleStateInput(self):
383 |         self.zt = np.dot(self.Succ_SS_PointSelectedTot, self.lambd)
384 |         self.zt_u = np.dot(self.Succ_uSS_PointSelectedTot, self.lambd)
385 | 
386 |     def addTerminalComponents(self,x0):
387 |         """add terminal constraint and terminal cost
388 |         Arguments:
389 |             x: initial condition
390 |         """        
391 |         # Update zt and xLin is they have crossed the finish line. We want s \in [0, TrackLength]
392 |         if (self.zt[4]-x0[4] > self.predictiveModel.map.TrackLength/2):
393 |             self.zt[4] = np.max([self.zt[4] - self.predictiveModel.map.TrackLength,0])
394 |             self.xLin[4,-1] = self.xLin[4,-1]- self.predictiveModel.map.TrackLength
395 |         sortedLapTime = np.argsort(np.array(self.LapTime))
396 | 
397 |         # Select Points from historical data. These points will be used to construct the terminal cost function and constraint set
398 |         SS_PointSelectedTot = np.empty((self.n, 0))
399 |         Succ_SS_PointSelectedTot = np.empty((self.n, 0))
400 |         Succ_uSS_PointSelectedTot = np.empty((self.d, 0))
401 |         Qfun_SelectedTot = np.empty((0))
402 |         for jj in sortedLapTime[0:self.numSS_it]:
403 |             SS_PointSelected, uSS_PointSelected, Qfun_Selected = self.selectPoints(jj, self.zt, self.numSS_Points / self.numSS_it + 1)
404 |             Succ_SS_PointSelectedTot =  np.append(Succ_SS_PointSelectedTot, SS_PointSelected[:,1:], axis=1)
405 |             Succ_uSS_PointSelectedTot =  np.append(Succ_uSS_PointSelectedTot, uSS_PointSelected[:,1:], axis=1)
406 |             SS_PointSelectedTot      = np.append(SS_PointSelectedTot, SS_PointSelected[:,0:-1], axis=1)
407 |             Qfun_SelectedTot         = np.append(Qfun_SelectedTot, Qfun_Selected[0:-1], axis=0)
408 | 
409 |         self.Succ_SS_PointSelectedTot = Succ_SS_PointSelectedTot
410 |         self.Succ_uSS_PointSelectedTot = Succ_uSS_PointSelectedTot
411 |         self.SS_PointSelectedTot = SS_PointSelectedTot
412 |         self.Qfun_SelectedTot = Qfun_SelectedTot
413 |         
414 |         # Update terminal set and cost
415 |         self.addSafeSetEqConstr()
416 |         self.addSafeSetCost()
417 | 
418 |     def addTrajectory(self, x, u, x_glob):
419 |         """update iteration index and construct SS, uSS and Qfun
420 |         Arguments:
421 |             x: closed-loop trajectory
422 |             u: applied inputs
423 |             x_gloab: closed-loop trajectory in global frame
424 |         """
425 |         self.LapTime.append(x.shape[0])
426 |         self.SS.append(x)
427 |         self.SS_glob.append(x_glob)
428 |         self.uSS.append(u)
429 |         self.Qfun.append(self.computeCost(x,u))
430 | 
431 |         if self.it == 0:
432 |             self.xLin = self.SS[self.it][1:self.N + 2, :]
433 |             self.uLin = self.uSS[self.it][1:self.N + 1, :]
434 | 
435 |         self.xStoredPredTraj.append(self.xStoredPredTraj_it)
436 |         self.xStoredPredTraj_it = []
437 | 
438 |         self.uStoredPredTraj.append(self.uStoredPredTraj_it)
439 |         self.uStoredPredTraj_it = []
440 | 
441 |         self.SSStoredPredTraj.append(self.SSStoredPredTraj_it)
442 |         self.SSStoredPredTraj_it = []
443 | 
444 |         self.it = self.it + 1
445 |         self.timeStep = 0
446 | 
447 |     def computeCost(self, x, u):
448 |         """compute roll-out cost
449 |         Arguments:
450 |             x: closed-loop trajectory
451 |             u: applied inputs
452 |         """
453 |         Cost = 10000 * np.ones((x.shape[0]))  # The cost has the same elements of the vector x --> time +1
454 |         # Now compute the cost moving backwards in a Dynamic Programming (DP) fashion.
455 |         # We start from the last element of the vector x and we sum the running cost
456 |         for i in range(0, x.shape[0]):
457 |             if (i == 0):  # Note that for i = 0 --> pick the latest element of the vector x
458 |                 Cost[x.shape[0] - 1 - i] = 0
459 |             elif x[x.shape[0] - 1 - i, 4]< self.predictiveModel.map.TrackLength:
460 |                 Cost[x.shape[0] - 1 - i] = Cost[x.shape[0] - 1 - i + 1] + 1
461 |             else:
462 |                 Cost[x.shape[0] - 1 - i] = 0
463 | 
464 |         return Cost
465 | 
466 |     def addPoint(self, x, u):
467 |         """at iteration j add the current point to SS, uSS and Qfun of the previous iteration
468 |         Arguments:
469 |             x: current state
470 |             u: current input
471 |         """
472 |         self.SS[self.it - 1]  = np.append(self.SS[self.it - 1], np.array([x + np.array([0, 0, 0, 0, self.predictiveModel.map.TrackLength, 0])]), axis=0)
473 |         self.uSS[self.it - 1] = np.append(self.uSS[self.it - 1], np.array([u]),axis=0)
474 |         self.Qfun[self.it - 1] = np.append(self.Qfun[self.it - 1], self.Qfun[self.it - 1][-1]-1)
475 |         # The above two lines are needed as the once the predicted trajectory has crossed the finish line the goal is
476 |         # to reach the end of the lap which is about to start
477 | 
478 |     def selectPoints(self, it, zt, numPoints):
479 |         """selecte (numPoints)-nearest neivbor to zt. These states will be used to construct the safe set and the value function approximation
480 |         Arguments:
481 |             x: current state
482 |             u: current input
483 |         """
484 |         x = self.SS[it]
485 |         u = self.uSS[it]
486 |         oneVec = np.ones((x.shape[0], 1))
487 |         x0Vec = (np.dot(np.array([zt]).T, oneVec.T)).T
488 |         diff = x - x0Vec
489 |         norm = la.norm(diff, 1, axis=1)
490 |         MinNorm = np.argmin(norm)
491 | 
492 |         if (MinNorm - numPoints/2 >= 0):
493 |             indexSSandQfun = range(-int(numPoints/2) + MinNorm, int(numPoints/2) + MinNorm + 1)
494 |         else:
495 |             indexSSandQfun = range(MinNorm, MinNorm + int(numPoints))
496 | 
497 |         SS_Points  = x[indexSSandQfun, :].T
498 |         SSu_Points = u[indexSSandQfun, :].T
499 |         Sel_Qfun = self.Qfun[it][indexSSandQfun]
500 | 
501 |         # Modify the cost if the predicion has crossed the finisch line
502 |         if self.xPred == []:
503 |             Sel_Qfun = self.Qfun[it][indexSSandQfun]
504 |         elif (np.all((self.xPred[:, 4] > self.predictiveModel.map.TrackLength) == False)):
505 |             Sel_Qfun = self.Qfun[it][indexSSandQfun]
506 |         elif it < self.it - 1:
507 |             Sel_Qfun = self.Qfun[it][indexSSandQfun] + self.Qfun[it][0]
508 |         else:
509 |             sPred = self.xPred[:, 4]
510 |             predCurrLap = self.N - sum(sPred > self.predictiveModel.map.TrackLength)
511 |             currLapTime = self.timeStep
512 |             Sel_Qfun = self.Qfun[it][indexSSandQfun] + currLapTime + predCurrLap
513 | 
514 |         return SS_Points, SSu_Points, Sel_Qfun


--------------------------------------------------------------------------------
/src/fnc/controller/PredictiveModel.py:
--------------------------------------------------------------------------------
  1 | from cvxopt import spmatrix, matrix, solvers
  2 | from numpy import linalg as la
  3 | from cvxopt.solvers import qp
  4 | import numpy as np
  5 | import datetime
  6 | import pdb
  7 | # This class is not generic and is tailored to the autonomous racing problem.
  8 | # The only method need the LT-MPC and the LMPC is regressionAndLinearization, which given a state-action pair
  9 | # compute the matrices A,B,C such that x_{k+1} = A x_k + Bu_k + C
 10 | 
 11 | class PredictiveModel():
 12 |     def __init__(self,  n, d, map, trToUse):
 13 |         self.map = map
 14 |         self.n = n # state dimension
 15 |         self.d = d # input dimention
 16 |         self.xStored = []
 17 |         self.uStored = []
 18 |         self.MaxNumPoint = 7 # max number of point per lap to use 
 19 |         self.h = 5 # bandwidth of the Kernel for local linear regression
 20 |         self.lamb = 0.0 # regularization
 21 |         self.dt = 0.1
 22 |         self.scaling = np.array([[0.1, 0.0, 0.0, 0.0, 0.0],
 23 |                                 [0.0, 1.0, 0.0, 0.0, 0.0],
 24 |                                 [0.0, 0.0, 1.0, 0.0, 0.0],
 25 |                                 [0.0, 0.0, 0.0, 1.0, 0.0],
 26 |                                 [0.0, 0.0, 0.0, 0.0, 1.0]])
 27 | 
 28 |         self.stateFeatures    = [0, 1, 2]
 29 |         self.inputFeaturesVx  = [1]
 30 |         self.inputFeaturesLat = [0]
 31 |         self.usedIt = [i for i in range(trToUse)]
 32 |         self.lapTime = []
 33 |     
 34 | 
 35 |     def addTrajectory(self, x, u):
 36 |         if self.lapTime == [] or x.shape[0] >= self.lapTime[-1]:
 37 |             self.xStored.append(x)
 38 |             self.uStored.append(u)
 39 |             self.lapTime.append(x.shape[0])
 40 |         else:
 41 |             for i in range(0, len(self.xStored)):
 42 |                 if x.shape[0] < self.lapTime[i]:
 43 |                     self.xStored.insert(i, x) 
 44 |                     self.uStored.insert(i, u) 
 45 |                     self.lapTime.insert(i, x.shape[0]) 
 46 |                     break
 47 | 
 48 |     def regressionAndLinearization(self, x, u):
 49 |         Ai = np.zeros((self.n, self.n))
 50 |         Bi = np.zeros((self.n, self.d))
 51 |         Ci = np.zeros(self.n)
 52 | 
 53 |         # Compute Index to use for each stored lap
 54 |         xuLin = np.hstack((x[self.stateFeatures], u[:]))
 55 |         self.indexSelected = []
 56 |         self.K = []
 57 |         for ii in self.usedIt:
 58 |             indexSelected_i, K_i = self.computeIndices(xuLin, ii)
 59 |             self.indexSelected.append(indexSelected_i)
 60 |             self.K.append(K_i)
 61 |         # print("xuLin: ",xuLin)
 62 |         # print("aaa indexSelected: ", self.indexSelected)
 63 | 
 64 |         # =========================
 65 |         # ====== Identify vx ======
 66 |         Q_vx, M_vx = self.compute_Q_M(self.inputFeaturesVx, self.usedIt)
 67 | 
 68 |         yIndex = 0
 69 |         b_vx = self.compute_b(yIndex, self.usedIt, M_vx)
 70 |         Ai[yIndex, self.stateFeatures], Bi[yIndex, self.inputFeaturesVx], Ci[yIndex] = self.LMPC_LocLinReg(Q_vx, b_vx, self.inputFeaturesVx)
 71 | 
 72 |         # =======================================
 73 |         # ====== Identify Lateral Dynamics ======
 74 |         Q_lat, M_lat = self.compute_Q_M(self.inputFeaturesLat, self.usedIt)
 75 | 
 76 |         yIndex = 1  # vy
 77 |         b_vy = self.compute_b(yIndex, self.usedIt, M_lat)
 78 |         Ai[yIndex, self.stateFeatures], Bi[yIndex, self.inputFeaturesLat], Ci[yIndex] = self.LMPC_LocLinReg(Q_lat, b_vy, self.inputFeaturesLat)
 79 | 
 80 |         yIndex = 2  # wz
 81 |         b_wz = self.compute_b(yIndex, self.usedIt, M_lat)
 82 |         Ai[yIndex, self.stateFeatures], Bi[yIndex, self.inputFeaturesLat], Ci[yIndex] = self.LMPC_LocLinReg(Q_lat, b_wz, self.inputFeaturesLat)
 83 | 
 84 |         # ===========================
 85 |         # ===== Linearization =======
 86 |         vx = x[0]; vy   = x[1]
 87 |         wz = x[2]; epsi = x[3]
 88 |         s  = x[4]; ey   = x[5]
 89 |         dt = self.dt
 90 | 
 91 |         if s < 0:
 92 |             print("s is negative, here the state: \n", x)
 93 | 
 94 |         startTimer = datetime.datetime.now()  # Start timer for LMPC iteration
 95 |         cur = self.map.curvature(s)
 96 |         cur = self.map.curvature(s)
 97 |         den = 1 - cur * ey
 98 | 
 99 |         # ===========================
100 |         # ===== Linearize epsi ======
101 |         # epsi_{k+1} = epsi + dt * ( wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) * cur )
102 |         depsi_vx   = -dt * np.cos(epsi) / den * cur
103 |         depsi_vy   = dt * np.sin(epsi) / den * cur
104 |         depsi_wz   = dt
105 |         depsi_epsi = 1 - dt * (-vx * np.sin(epsi) - vy * np.cos(epsi)) / den * cur
106 |         depsi_s    = 0  # Because cur = constant
107 |         depsi_ey   = dt * (vx * np.cos(epsi) - vy * np.sin(epsi)) / (den ** 2) * cur * (-cur)
108 | 
109 |         Ai[3, :] = [depsi_vx, depsi_vy, depsi_wz, depsi_epsi, depsi_s, depsi_ey]
110 |         Ci[3]    = epsi + dt * (wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) * cur) - np.dot(Ai[3, :], x)
111 |         # ===========================
112 |         # ===== Linearize s =========
113 |         # s_{k+1} = s    + dt * ( (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) )
114 |         ds_vx   = dt * (np.cos(epsi) / den)
115 |         ds_vy   = -dt * (np.sin(epsi) / den)
116 |         ds_wz   = 0
117 |         ds_epsi = dt * (-vx * np.sin(epsi) - vy * np.cos(epsi)) / den
118 |         ds_s    = 1  # + Ts * (Vx * cos(epsi) - Vy * sin(epsi)) / (1 - ey * rho) ^ 2 * (-ey * drho);
119 |         ds_ey   = -dt * (vx * np.cos(epsi) - vy * np.sin(epsi)) / (den ** 2) * (-cur)
120 | 
121 |         Ai[4, :] = [ds_vx, ds_vy, ds_wz, ds_epsi, ds_s, ds_ey]
122 |         Ci[4]    = s + dt * ((vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey)) - np.dot(Ai[4, :], x)
123 | 
124 |         # ===========================
125 |         # ===== Linearize ey ========
126 |         # ey_{k+1} = ey + dt * (vx * np.sin(epsi) + vy * np.cos(epsi))
127 |         dey_vx   = dt * np.sin(epsi)
128 |         dey_vy   = dt * np.cos(epsi)
129 |         dey_wz   = 0
130 |         dey_epsi = dt * (vx * np.cos(epsi) - vy * np.sin(epsi))
131 |         dey_s    = 0
132 |         dey_ey   = 1
133 | 
134 |         Ai[5, :] = [dey_vx, dey_vy, dey_wz, dey_epsi, dey_s, dey_ey]
135 |         Ci[5]    = ey + dt * (vx * np.sin(epsi) + vy * np.cos(epsi)) - np.dot(Ai[5, :], x)
136 | 
137 |         endTimer = datetime.datetime.now(); deltaTimer_tv = endTimer - startTimer
138 | 
139 |         return Ai, Bi, Ci
140 | 
141 |     def compute_Q_M(self, inputFeatures, usedIt):
142 |         Counter = 0
143 |         X0   = np.empty((0,len(self.stateFeatures)+len(inputFeatures)))
144 |         Ktot = np.empty((0))
145 | 
146 |         for it in usedIt:
147 |             X0 = np.append( X0, np.hstack((self.xStored[it][np.ix_(self.indexSelected[Counter], self.stateFeatures)],self.uStored[it][np.ix_(self.indexSelected[Counter], inputFeatures)])), axis=0 )
148 |             Ktot    = np.append(Ktot, self.K[Counter])
149 |             Counter += 1
150 | 
151 |         M = np.hstack( (X0, np.ones((X0.shape[0], 1))) )
152 |         Q0 = np.dot(np.dot(M.T, np.diag(Ktot)), M)
153 |         Q = matrix(Q0 + self.lamb * np.eye(Q0.shape[0]))
154 | 
155 |         return Q, M
156 | 
157 |     def compute_b(self, yIndex, usedIt, M):
158 |         Counter = 0
159 |         y = np.empty((0))
160 |         Ktot = np.empty((0))
161 | 
162 |         for it in usedIt:
163 |             y       = np.append(y, np.squeeze(self.xStored[it][self.indexSelected[Counter] + 1, yIndex]))
164 |             Ktot    = np.append(Ktot, self.K[Counter])
165 |             Counter += 1
166 | 
167 |         b = matrix(-np.dot(np.dot(M.T, np.diag(Ktot)), y))
168 |         return b
169 | 
170 |     def LMPC_LocLinReg(self, Q, b, inputFeatures):
171 |         # Solve QP
172 |         res_cons = qp(Q, b) # This is ordered as [A B C]
173 |         # Unpack results
174 |         result = np.squeeze(np.array(res_cons['x']))
175 |         A = result[0:len(self.stateFeatures)]
176 |         B = result[len(self.stateFeatures):(len(self.stateFeatures)+len(inputFeatures))]
177 |         C = result[-1]
178 |         return A, B, C
179 | 
180 |     def computeIndices(self, x, it):
181 |         oneVec = np.ones( (self.xStored[it].shape[0]-1, 1) )
182 |         xVec = (np.dot( np.array([x]).T, oneVec.T )).T
183 |         DataMatrix = np.hstack((self.xStored[it][0:-1, self.stateFeatures], self.uStored[it][0:-1, :]))
184 | 
185 |         diff  = np.dot(( DataMatrix - xVec ), self.scaling)
186 |         norm = la.norm(diff, 1, axis=1)
187 |         indexTot =  np.squeeze(np.where(norm < self.h))
188 |         if (indexTot.shape[0] >= self.MaxNumPoint):
189 |             index = np.argsort(norm)[0:self.MaxNumPoint]
190 |         else:
191 |             index = indexTot
192 | 
193 |         K  = ( 1 - ( norm[index] / self.h )**2 ) * 3/4
194 |         # if norm.shape[0]<500:
195 |         #     print("norm: ", norm, norm.shape)
196 | 
197 |         return index, K


--------------------------------------------------------------------------------
/src/fnc/plot.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import matplotlib.pyplot as plt
  3 | from matplotlib.animation import FuncAnimation
  4 | import matplotlib.patches as patches
  5 | 
  6 | import pdb
  7 | 
  8 | def plotTrajectory(map, x, x_glob, u, stringTitle):
  9 |     Points = int(np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4])))
 10 |     Points1 = np.zeros((Points, 2))
 11 |     Points2 = np.zeros((Points, 2))
 12 |     Points0 = np.zeros((Points, 2))
 13 |     for i in range(0, int(Points)):
 14 |         Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth)
 15 |         Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth)
 16 |         Points0[i, :] = map.getGlobalPosition(i * 0.1, 0)
 17 | 
 18 |     plt.figure()
 19 |     plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o')
 20 |     plt.plot(Points0[:, 0], Points0[:, 1], '--')
 21 |     plt.plot(Points1[:, 0], Points1[:, 1], '-b')
 22 |     plt.plot(Points2[:, 0], Points2[:, 1], '-b')
 23 |     plt.plot(x_glob[:, 4], x_glob[:, 5], '-r')
 24 |     plt.title(stringTitle)
 25 | 
 26 |     plt.figure()
 27 |     plt.subplot(711)
 28 |     plt.plot(x[:, 4], x[:, 0], '-o')
 29 |     plt.ylabel('vx')
 30 |     plt.subplot(712)
 31 |     plt.plot(x[:, 4], x[:, 1], '-o')
 32 |     plt.ylabel('vy')
 33 |     plt.subplot(713)
 34 |     plt.plot(x[:, 4], x[:, 2], '-o')
 35 |     plt.ylabel('wz')
 36 |     plt.subplot(714)
 37 |     plt.plot(x[:, 4], x[:, 3], '-o')
 38 |     plt.ylabel('epsi')
 39 |     plt.subplot(715)
 40 |     plt.plot(x[:, 4], x[:, 5], '-o')
 41 |     plt.ylabel('ey')
 42 |     plt.subplot(716)
 43 |     plt.plot(x[:, 4], u[:, 0], '-o')
 44 |     plt.ylabel('steering')
 45 |     plt.subplot(717)
 46 |     plt.plot(x[:, 4], u[:, 1], '-o')
 47 |     plt.ylabel('acc')
 48 |     plt.title(stringTitle)
 49 | 
 50 | def plotClosedLoopLMPC(lmpc, map):
 51 |     SS_glob = lmpc.SS_glob
 52 |     LapTime  = lmpc.LapTime
 53 |     SS      = lmpc.SS
 54 |     uSS     = lmpc.uSS
 55 | 
 56 |     TotNumberIt = lmpc.it
 57 |     Points = int(np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4])))
 58 |     Points1 = np.zeros((Points, 2))
 59 |     Points2 = np.zeros((Points, 2))
 60 |     Points0 = np.zeros((Points, 2))
 61 |     for i in range(0, int(Points)):
 62 |         Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth)
 63 |         Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth)
 64 |         Points0[i, :] = map.getGlobalPosition(i * 0.1, 0)
 65 | 
 66 |     plt.figure()
 67 |     plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o')
 68 |     plt.plot(Points0[:, 0], Points0[:, 1], '--')
 69 |     plt.plot(Points1[:, 0], Points1[:, 1], '-b')
 70 |     plt.plot(Points2[:, 0], Points2[:, 1], '-b')
 71 | 
 72 |     for i in range(TotNumberIt-5, TotNumberIt):
 73 |         plt.plot(SS_glob[i][0:LapTime[i], 4], SS_glob[i][0:LapTime[i], 5], '-r')
 74 | 
 75 |     plt.figure()
 76 |     plt.subplot(711)
 77 |     for i in range(2, TotNumberIt):
 78 |         plt.plot(SS[i][0:LapTime[i], 4], SS[i][0:LapTime[i], 0], '-o')
 79 |     plt.ylabel('vx')
 80 |     plt.subplot(712)
 81 |     for i in range(2, TotNumberIt):
 82 |         plt.plot(SS[i][0:LapTime[i], 4], SS[i][0:LapTime[i], 1], '-o')
 83 |     plt.ylabel('vy')
 84 |     plt.subplot(713)
 85 |     for i in range(2, TotNumberIt):
 86 |         plt.plot(SS[i][0:LapTime[i], 4], SS[i][0:LapTime[i], 2], '-o')
 87 |     plt.ylabel('wz')
 88 |     plt.subplot(714)
 89 |     for i in range(2, TotNumberIt):
 90 |         plt.plot(SS[i][0:LapTime[i], 4], SS[i][0:LapTime[i], 3], '-o')
 91 |     plt.ylabel('epsi')
 92 |     plt.subplot(715)
 93 |     for i in range(2, TotNumberIt):
 94 |         plt.plot(SS[i][0:LapTime[i], 4], SS[i][0:LapTime[i], 5], '-o')
 95 |     plt.ylabel('ey')
 96 |     plt.subplot(716)
 97 |     for i in range(2, TotNumberIt):
 98 |         plt.plot(uSS[i][0:LapTime[i] - 1, 0], '-o')
 99 |     plt.ylabel('Steering')
100 |     plt.subplot(717)
101 |     for i in range(2, TotNumberIt):
102 |         plt.plot(uSS[i][0:LapTime[i] - 1, 1], '-o')
103 |     plt.ylabel('Acc')
104 | 
105 | 
106 | def animation_xy(map, lmpc, it):
107 |     SS_glob = lmpc.SS_glob
108 |     LapTime = lmpc.LapTime
109 |     SS = lmpc.SS
110 |     uSS = lmpc.uSS
111 | 
112 |     Points = int(np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4])))
113 |     Points1 = np.zeros((Points, 2))
114 |     Points2 = np.zeros((Points, 2))
115 |     Points0 = np.zeros((Points, 2))
116 | 
117 |     for i in range(0, int(Points)):
118 |         Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth)
119 |         Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth)
120 |         Points0[i, :] = map.getGlobalPosition(i * 0.1, 0)
121 | 
122 |     plt.figure(200)
123 |     plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o')
124 |     plt.plot(Points0[:, 0], Points0[:, 1], '--')
125 |     plt.plot(Points1[:, 0], Points1[:, 1], '-b')
126 |     plt.plot(Points2[:, 0], Points2[:, 1], '-b')
127 |     plt.plot(SS_glob[it][0:LapTime[it], 4], SS_glob[it][0:LapTime[it], 5], '-ok', label="Closed-loop trajectory",zorder=-1)
128 | 
129 |     ax = plt.axes()
130 |     SSpoints_x = []; SSpoints_y = []
131 |     xPred = []; yPred = []
132 |     SSpoints, = ax.plot(SSpoints_x, SSpoints_y, 'sb', label="SS",zorder=0)
133 |     line, = ax.plot(xPred, yPred, '-or', label="Predicted Trajectory",zorder=1)
134 | 
135 |     v = np.array([[ 1.,  1.],
136 |                   [ 1., -1.],
137 |                   [-1., -1.],
138 |                   [-1.,  1.]])
139 |     rec = patches.Polygon(v, alpha=0.7,closed=True, fc='r', ec='k',zorder=10)
140 |     ax.add_patch(rec)
141 | 
142 |     plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower left",
143 |                 mode="expand", borderaxespad=0, ncol=3)
144 | 
145 | 
146 |     N = lmpc.N
147 |     numSS_Points = lmpc.numSS_Points
148 |     for i in range(0, int(lmpc.LapTime[it])):
149 | 
150 |         xPred = np.zeros((N+1, 1)); yPred = np.zeros((N+1, 1))
151 |         SSpoints_x = np.zeros((numSS_Points, 1)); SSpoints_y = np.zeros((numSS_Points, 1))
152 |         
153 |         for j in range(0, N+1):
154 |             xPred[j,0], yPred[j,0]  = map.getGlobalPosition( lmpc.xStoredPredTraj[it][i][j, 4],
155 |                                                              lmpc.xStoredPredTraj[it][i][j, 5] )
156 | 
157 |             if j == 0:
158 |                 x = SS_glob[it][i, 4]
159 |                 y = SS_glob[it][i, 5]
160 |                 psi = SS_glob[it][i, 3]
161 |                 l = 0.4; w = 0.2
162 |                 car_x = [ x + l * np.cos(psi) - w * np.sin(psi), x + l*np.cos(psi) + w * np.sin(psi),
163 |                           x - l * np.cos(psi) + w * np.sin(psi), x - l * np.cos(psi) - w * np.sin(psi)]
164 |                 car_y = [ y + l * np.sin(psi) + w * np.cos(psi), y + l * np.sin(psi) - w * np.cos(psi),
165 |                           y - l * np.sin(psi) - w * np.cos(psi), y - l * np.sin(psi) + w * np.cos(psi)]
166 | 
167 |         for j in range(0, numSS_Points):
168 |             SSpoints_x[j,0], SSpoints_y[j,0] = map.getGlobalPosition(lmpc.SSStoredPredTraj[it][i][j, 4],
169 |                                                                      lmpc.SSStoredPredTraj[it][i][j, 5])
170 |         SSpoints.set_data(SSpoints_x, SSpoints_y)
171 | 
172 |         line.set_data(xPred, yPred)
173 |         rec.set_xy(np.array([car_x, car_y]).T)
174 |         plt.draw()
175 |         plt.pause(1e-17)
176 | 
177 | def animation_states(map, LMPCOpenLoopData, lmpc, it):
178 |     SS_glob = lmpc.SS_glob
179 |     LapTime = lmpc.LapTime
180 |     SS = lmpc.SS
181 |     uSS = lmpc.uSS
182 | 
183 |     xdata = []; ydata = []
184 |     fig = plt.figure(100)
185 | 
186 |     axvx = fig.add_subplot(3, 2, 1)
187 |     plt.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 0, it], '-ok', label="Closed-loop trajectory")
188 |     lineSSvx, = axvx.plot(xdata, ydata, 'sb-', label="SS")
189 |     linevx, = axvx.plot(xdata, ydata, 'or-', label="Predicted Trajectory")
190 |     plt.ylabel("vx")
191 |     plt.xlabel("s")
192 | 
193 |     plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower left",
194 |                 mode="expand", borderaxespad=0, ncol=3)
195 | 
196 |     axvy = fig.add_subplot(3, 2, 2)
197 |     axvy.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 1, it], '-ok')
198 |     lineSSvy, = axvy.plot(xdata, ydata, 'sb-')
199 |     linevy, = axvy.plot(xdata, ydata, 'or-')
200 |     plt.ylabel("vy")
201 |     plt.xlabel("s")
202 | 
203 |     axwz = fig.add_subplot(3, 2, 3)
204 |     axwz.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 2, it], '-ok')
205 |     lineSSwz, = axwz.plot(xdata, ydata, 'sb-')
206 |     linewz, = axwz.plot(xdata, ydata, 'or-')
207 |     plt.ylabel("wz")
208 |     plt.xlabel("s")
209 | 
210 |     axepsi = fig.add_subplot(3, 2, 4)
211 |     axepsi.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 3, it], '-ok')
212 |     lineSSepsi, = axepsi.plot(xdata, ydata, 'sb-')
213 |     lineepsi, = axepsi.plot(xdata, ydata, 'or-')
214 |     plt.ylabel("epsi")
215 |     plt.xlabel("s")
216 | 
217 |     axey = fig.add_subplot(3, 2, 5)
218 |     axey.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 5, it], '-ok')
219 |     lineSSey, = axey.plot(xdata, ydata, 'sb-')
220 |     lineey, = axey.plot(xdata, ydata, 'or-')
221 |     plt.ylabel("ey")
222 |     plt.xlabel("s")
223 | 
224 |     Points = np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4]))
225 |     Points1 = np.zeros((Points, 2))
226 |     Points2 = np.zeros((Points, 2))
227 |     Points0 = np.zeros((Points, 2))
228 |     for i in range(0, int(Points)):
229 |         Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth)
230 |         Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth)
231 |         Points0[i, :] = map.getGlobalPosition(i * 0.1, 0)
232 | 
233 |     axtr = fig.add_subplot(3, 2, 6)
234 |     plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o')
235 |     plt.plot(Points0[:, 0], Points0[:, 1], '--')
236 |     plt.plot(Points1[:, 0], Points1[:, 1], '-b')
237 |     plt.plot(Points2[:, 0], Points2[:, 1], '-b')
238 |     plt.plot(SS_glob[it][0:LapTime[it], 4], SS_glob[it][0:LapTime[it], 5], '-ok')
239 | 
240 |     SSpoints_x = []; SSpoints_y = []
241 |     xPred = []; yPred = []
242 |     SSpoints_tr, = axtr.plot(SSpoints_x, SSpoints_y, 'sb')
243 |     line_tr, = axtr.plot(xPred, yPred, '-or')
244 | 
245 |     N = lmpc.N
246 |     numSS_Points = lmpc.numSS_Points
247 |     for i in range(0, int(lmpc.LapTime[it])):
248 | 
249 |         xPred    = lmpc.xStoredPredTraj[it][i]
250 |         SSpoints = lmpc.SSStoredPredTraj[it][i]
251 | 
252 |         linevx.set_data(xPred[:, 4], xPred[:, 0]);   axvx.set_title(str(xPred[0, 0]))
253 |         linevy.set_data(xPred[:, 4], xPred[:, 1]);   axvy.set_title(str(xPred[0, 1]))
254 |         linewz.set_data(xPred[:, 4], xPred[:, 2]);   axwz.set_title(str(xPred[0, 2]))
255 |         lineepsi.set_data(xPred[:, 4], xPred[:, 3]); axepsi.set_title(str(xPred[0, 3]))
256 |         lineey.set_data(xPred[:, 4], xPred[:, 5]);   axey.set_title(str(xPred[0, 5]))
257 | 
258 |         epsiReal = xPred[0, 3]
259 | 
260 |         lineSSvx.set_data(SSpoints[4,:], SSpoints[0,:])
261 |         lineSSvy.set_data(SSpoints[4,:], SSpoints[1,:])
262 |         lineSSwz.set_data(SSpoints[4,:], SSpoints[2,:])
263 |         lineSSepsi.set_data(SSpoints[4,:], SSpoints[3,:])
264 |         lineSSey.set_data(SSpoints[4,:], SSpoints[5,:])
265 | 
266 |         xPred = np.zeros((N + 1, 1));yPred = np.zeros((N + 1, 1))
267 |         SSpoints_x = np.zeros((numSS_Points, 1));SSpoints_y = np.zeros((numSS_Points, 1))
268 | 
269 |         for j in range(0, N + 1):
270 |             xPred[j, 0], yPred[j, 0] = map.getGlobalPosition(lmpc.xStoredPredTraj[it][i][j, 4],
271 |                                                              lmpc.xStoredPredTraj[it][i][j, 5])
272 | 
273 |         for j in range(0, numSS_Points):
274 |             SSpoints_x[j, 0], SSpoints_y[j, 0] = map.getGlobalPosition(LMPCOpenLoopData.SSused[4, j, i, it],
275 |                                                                        LMPCOpenLoopData.SSused[5, j, i, it])
276 | 
277 |         line_tr.set_data(xPred, yPred)
278 | 
279 | 
280 |         vec = np.array([xPred[0, 0], yPred[0, 0]]) - np.array([SS_glob[i, 4, it], SS_glob[i, 5, it]])
281 | 
282 |         s, ey, epsi, _ = map.getLocalPosition( SS_glob[i, 4, it], SS_glob[i, 5, it], SS_glob[i, 3, it])
283 |         axtr.set_title(str(s)+" "+str(ey)+" "+str(epsi))
284 | 
285 |         # axepsi.set_title(str(epsiReal)+" "+str(epsi))
286 |         SSpoints_tr.set_data(SSpoints_x, SSpoints_y)
287 | 
288 |         plt.draw()
289 |         plt.pause(1e-17)
290 | 
291 | def saveGif_xyResults(map, LMPCOpenLoopData, lmpc, it):
292 |     SS_glob = lmpc.SS_glob
293 |     LapTime = lmpc.LapTime
294 |     SS = lmpc.SS
295 |     uSS = lmpc.uSS
296 | 
297 |     Points = int(np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4])))
298 |     Points1 = np.zeros((Points, 2))
299 |     Points2 = np.zeros((Points, 2))
300 |     Points0 = np.zeros((Points, 2))
301 |     for i in range(0, int(Points)):
302 |         Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth)
303 |         Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth)
304 |         Points0[i, :] = map.getGlobalPosition(i * 0.1, 0)
305 | 
306 |     fig = plt.figure(101)
307 |     # plt.ylim((-5, 1.5))
308 |     fig.set_tight_layout(True)
309 |     plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o')
310 |     plt.plot(Points0[:, 0], Points0[:, 1], '--')
311 |     plt.plot(Points1[:, 0], Points1[:, 1], '-b')
312 |     plt.plot(Points2[:, 0], Points2[:, 1], '-b')
313 |     plt.plot(SS_glob[it][0:LapTime[it], 4], SS_glob[it][0:LapTime[it], 5], '-ok', label="Closed-loop trajectory", markersize=1,zorder=-1)
314 | 
315 |     ax = plt.axes()
316 |     SSpoints_x = []; SSpoints_y = []
317 |     xPred = []; yPred = []
318 |     SSpoints, = ax.plot(SSpoints_x, SSpoints_y, 'og', label="SS",zorder=0)
319 |     line, = ax.plot(xPred, yPred, '-or', label="Predicted Trajectory",zorder=1)
320 | 
321 |     v = np.array([[ 1.,  1.],
322 |                   [ 1., -1.],
323 |                   [-1., -1.],
324 |                   [-1.,  1.]])
325 |     rec = patches.Polygon(v, alpha=0.7,closed=True, fc='g', ec='k',zorder=10)
326 |     ax.add_patch(rec)
327 | 
328 |     plt.legend(mode="expand", ncol=3)
329 |     plt.title('Lap: '+str(it))
330 |     # plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower left",
331 |     #             mode="expand", borderaxespad=0, ncol=3)
332 | 
333 |     N = lmpc.N
334 |     numSS_Points = lmpc.numSS_Points
335 | 
336 |     def update(i):
337 |         xPred = np.zeros((N + 1, 1)); yPred = np.zeros((N + 1, 1))
338 |         SSpoints_x = np.zeros((numSS_Points, 1)); SSpoints_y = np.zeros((numSS_Points, 1))
339 | 
340 |         for j in range(0, N + 1):
341 |             xPred[j, 0], yPred[j, 0] = map.getGlobalPosition(lmpc.xStoredPredTraj[it][i][j, 4],
342 |                                                              lmpc.xStoredPredTraj[it][i][j, 5])
343 | 
344 |             if j == 0:
345 |                 x = SS_glob[i, 4, it]
346 |                 y = SS_glob[i, 5, it]
347 |                 psi = SS_glob[i, 3, it]
348 |                 l = 0.4;w = 0.2
349 |                 car_x = [x + l * np.cos(psi) - w * np.sin(psi), x + l * np.cos(psi) + w * np.sin(psi),
350 |                          x - l * np.cos(psi) + w * np.sin(psi), x - l * np.cos(psi) - w * np.sin(psi)]
351 |                 car_y = [y + l * np.sin(psi) + w * np.cos(psi), y + l * np.sin(psi) - w * np.cos(psi),
352 |                          y - l * np.sin(psi) - w * np.cos(psi), y - l * np.sin(psi) + w * np.cos(psi)]
353 | 
354 |         for j in range(0, numSS_Points):
355 |             SSpoints_x[j, 0], SSpoints_y[j, 0] = map.getGlobalPosition(LMPCOpenLoopData.SSused[4, j, i, it],
356 |                                                                        LMPCOpenLoopData.SSused[5, j, i, it])
357 |         SSpoints.set_data(SSpoints_x, SSpoints_y)
358 | 
359 |         line.set_data(xPred, yPred)
360 | 
361 |         rec.set_xy(np.array([car_x, car_y]).T)
362 | 
363 |     anim = FuncAnimation(fig, update, frames=np.arange(0, int(lmpc.LapTime[it])), interval=100)
364 | 
365 |     anim.save('ClosedLoop.gif', dpi=80, writer='imagemagick')
366 | 


--------------------------------------------------------------------------------
/src/fnc/simulator/SysModel.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pdb
  3 | import datetime
  4 | from Utilities import wrap
  5 | 
  6 | class Simulator():
  7 |     """Vehicle simulator
  8 |     Attributes:
  9 |         Sim: given a Controller object run closed-loop simulation and write the data in the ClosedLoopData object
 10 |     """
 11 |     def __init__(self, map, dt = 0.1, multiLap = True, flagLMPC = False):
 12 |         """Initialization
 13 |         map: map
 14 |         lap: number of laps to run. If set to 0 then the simulation is completed when ClosedLoopData is full
 15 |         flagLMPC: set to 0 for standart controller. Set to 1 for LMPC --> at iteration j add data to SS^{j-1} (look line 9999)
 16 |         """
 17 |         self.map = map
 18 |         self.multiLap = multiLap
 19 |         self.flagLMPC = flagLMPC
 20 |         self.dt = dt
 21 | 
 22 |     def sim(self, x0,  Controller, maxSimTime = 100):
 23 |         """Simulate closed-loop system
 24 |         """
 25 |         x_cl      = [x0[0]]
 26 |         x_cl_glob = [x0[1]]
 27 |         u_cl   = []
 28 |         sim_t  = 0
 29 |         SimulationTime = 0
 30 |         
 31 |         i=0
 32 |         flagExt = False
 33 |         while (i<int(maxSimTime/self.dt)) and (flagExt==False):
 34 |             Controller.solve(x_cl[-1])
 35 |             u_cl.append(Controller.uPred[0,:].copy())
 36 | 
 37 |             if self.flagLMPC == True:
 38 |                 Controller.addPoint(x_cl[-1], u_cl[-1])
 39 | 
 40 |             xt, xt_glob = self.dynModel(x_cl[-1], x_cl_glob[-1], u_cl[-1])
 41 |             
 42 |             x_cl.append(xt)
 43 |             x_cl_glob.append(xt_glob)
 44 | 
 45 |             if (self.multiLap == False) and ( x_cl[-1][4] > (self.map.TrackLength)):
 46 |                 print("Lap completed")
 47 |                 flagExt = True
 48 |             i += 1
 49 | 
 50 |         xF = [np.array(x_cl[-1]) - np.array([0, 0, 0, 0, self.map.TrackLength, 0]), np.array(x_cl_glob[-1])]
 51 |         x_cl.pop()
 52 |         x_cl_glob.pop()
 53 | 
 54 |         return np.array(x_cl), np.array(u_cl), np.array(x_cl_glob), xF
 55 | 
 56 |     def dynModel(self, x, x_glob, u):
 57 |         # This method computes the system evolution. Note that the discretization is deltaT and therefore is needed that
 58 |         # dt <= deltaT and ( dt / deltaT) = integer value
 59 | 
 60 |         # Vehicle Parameters
 61 |         m  = 1.98
 62 |         lf = 0.125
 63 |         lr = 0.125
 64 |         Iz = 0.024
 65 |         Df = 0.8 * m * 9.81 / 2.0
 66 |         Cf = 1.25
 67 |         Bf = 1.0
 68 |         Dr = 0.8 * m * 9.81 / 2.0
 69 |         Cr = 1.25
 70 |         Br = 1.0
 71 | 
 72 |         # Discretization Parameters
 73 |         deltaT = 0.001
 74 |         x_next     = np.zeros(x.shape[0])
 75 |         cur_x_next = np.zeros(x.shape[0])
 76 | 
 77 |         # Extract the value of the states
 78 |         delta = u[0]
 79 |         a     = u[1]
 80 | 
 81 |         psi = x_glob[3]
 82 |         X = x_glob[4]
 83 |         Y = x_glob[5]
 84 | 
 85 |         vx    = x[0]
 86 |         vy    = x[1]
 87 |         wz    = x[2]
 88 |         epsi  = x[3]
 89 |         s     = x[4]
 90 |         ey    = x[5]
 91 | 
 92 |         # Initialize counter
 93 |         i = 0
 94 |         while( (i+1) * deltaT <= self.dt):
 95 |             # Compute tire split angle
 96 |             alpha_f = delta - np.arctan2( vy + lf * wz, vx )
 97 |             alpha_r = - np.arctan2( vy - lf * wz , vx)
 98 | 
 99 |             # Compute lateral force at front and rear tire
100 |             Fyf = Df * np.sin( Cf * np.arctan(Bf * alpha_f ) )
101 |             Fyr = Dr * np.sin( Cr * np.arctan(Br * alpha_r ) )
102 | 
103 |             # Propagate the dynamics of deltaT
104 |             x_next[0] = vx  + deltaT * (a - 1 / m * Fyf * np.sin(delta) + wz*vy)
105 |             x_next[1] = vy  + deltaT * (1 / m * (Fyf * np.cos(delta) + Fyr) - wz * vx)
106 |             x_next[2] = wz  + deltaT * (1 / Iz *(lf * Fyf * np.cos(delta) - lr * Fyr) )
107 |             x_next[3] = psi + deltaT * (wz)
108 |             x_next[4] =   X + deltaT * ((vx * np.cos(psi) - vy * np.sin(psi)))
109 |             x_next[5] =   Y + deltaT * (vx * np.sin(psi)  + vy * np.cos(psi))
110 | 
111 |             cur = self.map.curvature(s)
112 |             cur_x_next[0] = vx   + deltaT * (a - 1 / m * Fyf * np.sin(delta) + wz*vy)
113 |             cur_x_next[1] = vy   + deltaT * (1 / m * (Fyf * np.cos(delta) + Fyr) - wz * vx)
114 |             cur_x_next[2] = wz   + deltaT * (1 / Iz *(lf * Fyf * np.cos(delta) - lr * Fyr) )
115 |             cur_x_next[3] = epsi + deltaT * ( wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) * cur )
116 |             cur_x_next[4] = s    + deltaT * ( (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) )
117 |             cur_x_next[5] = ey   + deltaT * (vx * np.sin(epsi) + vy * np.cos(epsi))
118 | 
119 |             # Update the value of the states
120 |             psi  = x_next[3]
121 |             X    = x_next[4]
122 |             Y    = x_next[5]
123 | 
124 |             vx   = cur_x_next[0]
125 |             vy   = cur_x_next[1]
126 |             wz   = cur_x_next[2]
127 |             epsi = cur_x_next[3]
128 |             s    = cur_x_next[4]
129 |             ey   = cur_x_next[5]
130 | 
131 |             if (s < 0):
132 |                 print("Start Point: ", x, " Input: ", u)
133 |                 print("x_next: ", x_next)
134 | 
135 |             # Increment counter
136 |             i = i+1
137 | 
138 |         # Noises
139 |         noise_vx = np.max([-0.05, np.min([np.random.randn() * 0.01, 0.05])])
140 |         noise_vy = np.max([-0.05, np.min([np.random.randn() * 0.01, 0.05])])
141 |         noise_wz = np.max([-0.05, np.min([np.random.randn() * 0.005, 0.05])])
142 | 
143 |         cur_x_next[0] = cur_x_next[0] + 0.01*noise_vx
144 |         cur_x_next[1] = cur_x_next[1] + 0.01*noise_vy
145 |         cur_x_next[2] = cur_x_next[2] + 0.01*noise_wz
146 | 
147 |         return cur_x_next, x_next
148 | 
149 | 
150 | 
151 | 


--------------------------------------------------------------------------------
/src/fnc/simulator/Track.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import numpy.linalg as la
  3 | import pdb
  4 | 
  5 | class Map():
  6 |     """map object
  7 |     Attributes:
  8 |         getGlobalPosition: convert position from (s, ey) to (X,Y)
  9 |     """
 10 |     def __init__(self, halfWidth):
 11 |         """Initialization
 12 |         halfWidth: track halfWidth
 13 |         Modify the vector spec to change the geometry of the track
 14 |         """
 15 |         # Goggle-shaped track
 16 |         # self.slack = 0.15
 17 |         # self.halfWidth = halfWidth
 18 |         # spec = np.array([[60 * 0.03, 0],
 19 |         #                  [80 * 0.03, -80 * 0.03 * 2 / np.pi],
 20 |         #                  # Note s = 1 * np.pi / 2 and r = -1 ---> Angle spanned = np.pi / 2
 21 |         #                  [20 * 0.03, 0],
 22 |         #                  [80 * 0.03, -80 * 0.03 * 2 / np.pi],
 23 |         #                  [40 * 0.03, +40 * 0.03 * 10 / np.pi],
 24 |         #                  [60 * 0.03, -60 * 0.03 * 5 / np.pi],
 25 |         #                  [40 * 0.03, +40 * 0.03 * 10 / np.pi],
 26 |         #                  [80 * 0.03, -80 * 0.03 * 2 / np.pi],
 27 |         #                  [20 * 0.03, 0],
 28 |         #                  [80 * 0.03, -80 * 0.03 * 2 / np.pi]])
 29 | 
 30 |         # L-shaped track
 31 |         self.halfWidth = 0.4
 32 |         self.slack = 0.45
 33 |         lengthCurve = 4.5
 34 |         spec = np.array([[1.0, 0],
 35 |                          [lengthCurve, lengthCurve / np.pi],
 36 |                          # Note s = 1 * np.pi / 2 and r = -1 ---> Angle spanned = np.pi / 2
 37 |                          [lengthCurve / 2, -lengthCurve / np.pi],
 38 |                          [lengthCurve, lengthCurve / np.pi],
 39 |                          [lengthCurve / np.pi * 2, 0],
 40 |                          [lengthCurve / 2, lengthCurve / np.pi]])
 41 | 
 42 | 
 43 |         # spec = np.array([[1.0, 0],
 44 |         #                  [4.5, -4.5 / np.pi],
 45 |         #                  # Note s = 1 * np.pi / 2 and r = -1 ---> Angle spanned = np.pi / 2
 46 |         #                  [2.0, 0],
 47 |         #                  [4.5, -4.5 / np.pi],
 48 |         #                  [1.0, 0]])
 49 | 
 50 |         # Now given the above segments we compute the (x, y) points of the track and the angle of the tangent vector (psi) at
 51 |         # these points. For each segment we compute the (x, y, psi) coordinate at the last point of the segment. Furthermore,
 52 |         # we compute also the cumulative s at the starting point of the segment at signed curvature
 53 |         # PointAndTangent = [x, y, psi, cumulative s, segment length, signed curvature]
 54 |         PointAndTangent = np.zeros((spec.shape[0] + 1, 6))
 55 |         for i in range(0, spec.shape[0]):
 56 |             if spec[i, 1] == 0.0:              # If the current segment is a straight line
 57 |                 l = spec[i, 0]                 # Length of the segments
 58 |                 if i == 0:
 59 |                     ang = 0                          # Angle of the tangent vector at the starting point of the segment
 60 |                     x = 0 + l * np.cos(ang)          # x coordinate of the last point of the segment
 61 |                     y = 0 + l * np.sin(ang)          # y coordinate of the last point of the segment
 62 |                 else:
 63 |                     ang = PointAndTangent[i - 1, 2]                 # Angle of the tangent vector at the starting point of the segment
 64 |                     x = PointAndTangent[i-1, 0] + l * np.cos(ang)  # x coordinate of the last point of the segment
 65 |                     y = PointAndTangent[i-1, 1] + l * np.sin(ang)  # y coordinate of the last point of the segment
 66 |                 psi = ang  # Angle of the tangent vector at the last point of the segment
 67 | 
 68 | 
 69 |                 if i == 0:
 70 |                     NewLine = np.array([x, y, psi, PointAndTangent[i, 3], l, 0])
 71 |                 else:
 72 |                     NewLine = np.array([x, y, psi, PointAndTangent[i-1, 3] + PointAndTangent[i-1, 4], l, 0])
 73 | 
 74 |                 PointAndTangent[i, :] = NewLine  # Write the new info
 75 |             else:
 76 |                 l = spec[i, 0]                 # Length of the segment
 77 |                 r = spec[i, 1]                 # Radius of curvature
 78 | 
 79 | 
 80 |                 if r >= 0:
 81 |                     direction = 1
 82 |                 else:
 83 |                     direction = -1
 84 | 
 85 |                 if i == 0:
 86 |                     ang = 0                                                      # Angle of the tangent vector at the
 87 |                                                                                  # starting point of the segment
 88 |                     CenterX = 0 \
 89 |                               + np.abs(r) * np.cos(ang + direction * np.pi / 2)  # x coordinate center of circle
 90 |                     CenterY = 0 \
 91 |                               + np.abs(r) * np.sin(ang + direction * np.pi / 2)  # y coordinate center of circle
 92 |                 else:
 93 |                     ang = PointAndTangent[i - 1, 2]                              # Angle of the tangent vector at the
 94 |                                                                                  # starting point of the segment
 95 |                     CenterX = PointAndTangent[i-1, 0] \
 96 |                               + np.abs(r) * np.cos(ang + direction * np.pi / 2)  # x coordinate center of circle
 97 |                     CenterY = PointAndTangent[i-1, 1] \
 98 |                               + np.abs(r) * np.sin(ang + direction * np.pi / 2)  # y coordinate center of circle
 99 | 
100 |                 spanAng = l / np.abs(r)  # Angle spanned by the circle
101 |                 psi = wrap(ang + spanAng * np.sign(r))  # Angle of the tangent vector at the last point of the segment
102 | 
103 |                 angleNormal = wrap((direction * np.pi / 2 + ang))
104 |                 angle = -(np.pi - np.abs(angleNormal)) * (sign(angleNormal))
105 |                 x = CenterX + np.abs(r) * np.cos(
106 |                     angle + direction * spanAng)  # x coordinate of the last point of the segment
107 |                 y = CenterY + np.abs(r) * np.sin(
108 |                     angle + direction * spanAng)  # y coordinate of the last point of the segment
109 | 
110 |                 if i == 0:
111 |                     NewLine = np.array([x, y, psi, PointAndTangent[i, 3], l, 1 / r])
112 |                 else:
113 |                     NewLine = np.array([x, y, psi, PointAndTangent[i-1, 3] + PointAndTangent[i-1, 4], l, 1 / r])
114 | 
115 |                 PointAndTangent[i, :] = NewLine  # Write the new info
116 |             # plt.plot(x, y, 'or')
117 | 
118 | 
119 |         xs = PointAndTangent[-2, 0]
120 |         ys = PointAndTangent[-2, 1]
121 |         xf = 0
122 |         yf = 0
123 |         psif = 0
124 | 
125 |         # plt.plot(xf, yf, 'or')
126 |         # plt.show()
127 |         l = np.sqrt((xf - xs) ** 2 + (yf - ys) ** 2)
128 | 
129 |         NewLine = np.array([xf, yf, psif, PointAndTangent[-2, 3] + PointAndTangent[-2, 4], l, 0])
130 |         PointAndTangent[-1, :] = NewLine
131 | 
132 |         self.PointAndTangent = PointAndTangent
133 |         self.TrackLength = PointAndTangent[-1, 3] + PointAndTangent[-1, 4]
134 | 
135 |     def getGlobalPosition(self, s, ey):
136 |         """coordinate transformation from curvilinear reference frame (e, ey) to inertial reference frame (X, Y)
137 |         (s, ey): position in the curvilinear reference frame
138 |         """
139 | 
140 |         # wrap s along the track
141 |         while (s > self.TrackLength):
142 |             s = s - self.TrackLength
143 | 
144 |         # Compute the segment in which system is evolving
145 |         PointAndTangent = self.PointAndTangent
146 | 
147 |         index = np.all([[s >= PointAndTangent[:, 3]], [s < PointAndTangent[:, 3] + PointAndTangent[:, 4]]], axis=0)
148 |         i = int(np.where(np.squeeze(index))[0])
149 | 
150 |         if PointAndTangent[i, 5] == 0.0:  # If segment is a straight line
151 |             # Extract the first final and initial point of the segment
152 |             xf = PointAndTangent[i, 0]
153 |             yf = PointAndTangent[i, 1]
154 |             xs = PointAndTangent[i - 1, 0]
155 |             ys = PointAndTangent[i - 1, 1]
156 |             psi = PointAndTangent[i, 2]
157 | 
158 |             # Compute the segment length
159 |             deltaL = PointAndTangent[i, 4]
160 |             reltaL = s - PointAndTangent[i, 3]
161 | 
162 |             # Do the linear combination
163 |             x = (1 - reltaL / deltaL) * xs + reltaL / deltaL * xf + ey * np.cos(psi + np.pi / 2)
164 |             y = (1 - reltaL / deltaL) * ys + reltaL / deltaL * yf + ey * np.sin(psi + np.pi / 2)
165 |         else:
166 |             r = 1 / PointAndTangent[i, 5]  # Extract curvature
167 |             ang = PointAndTangent[i - 1, 2]  # Extract angle of the tangent at the initial point (i-1)
168 |             # Compute the center of the arc
169 |             if r >= 0:
170 |                 direction = 1
171 |             else:
172 |                 direction = -1
173 | 
174 |             CenterX = PointAndTangent[i - 1, 0] \
175 |                       + np.abs(r) * np.cos(ang + direction * np.pi / 2)  # x coordinate center of circle
176 |             CenterY = PointAndTangent[i - 1, 1] \
177 |                       + np.abs(r) * np.sin(ang + direction * np.pi / 2)  # y coordinate center of circle
178 | 
179 |             spanAng = (s - PointAndTangent[i, 3]) / (np.pi * np.abs(r)) * np.pi
180 | 
181 |             angleNormal = wrap((direction * np.pi / 2 + ang))
182 |             angle = -(np.pi - np.abs(angleNormal)) * (sign(angleNormal))
183 | 
184 |             x = CenterX + (np.abs(r) - direction * ey) * np.cos(
185 |                 angle + direction * spanAng)  # x coordinate of the last point of the segment
186 |             y = CenterY + (np.abs(r) - direction * ey) * np.sin(
187 |                 angle + direction * spanAng)  # y coordinate of the last point of the segment
188 | 
189 |         return x, y
190 | 
191 |     def getLocalPosition(self, x, y, psi):
192 |         """coordinate transformation from inertial reference frame (X, Y) to curvilinear reference frame (s, ey)
193 |         (X, Y): position in the inertial reference frame
194 |         """
195 |         PointAndTangent = self.PointAndTangent
196 |         CompletedFlag = 0
197 | 
198 | 
199 | 
200 |         for i in range(0, PointAndTangent.shape[0]):
201 |             if CompletedFlag == 1:
202 |                 break
203 | 
204 |             if PointAndTangent[i, 5] == 0.0:  # If segment is a straight line
205 |                 # Extract the first final and initial point of the segment
206 |                 xf = PointAndTangent[i, 0]
207 |                 yf = PointAndTangent[i, 1]
208 |                 xs = PointAndTangent[i - 1, 0]
209 |                 ys = PointAndTangent[i - 1, 1]
210 | 
211 |                 psi_unwrap = np.unwrap([PointAndTangent[i - 1, 2], psi])[1]
212 |                 epsi = psi_unwrap - PointAndTangent[i - 1, 2]
213 |                 # Check if on the segment using angles
214 |                 if (la.norm(np.array([xs, ys]) - np.array([x, y]))) == 0:
215 |                     s  = PointAndTangent[i, 3]
216 |                     ey = 0
217 |                     CompletedFlag = 1
218 | 
219 |                 elif (la.norm(np.array([xf, yf]) - np.array([x, y]))) == 0:
220 |                     s = PointAndTangent[i, 3] + PointAndTangent[i, 4]
221 |                     ey = 0
222 |                     CompletedFlag = 1
223 |                 else:
224 |                     if np.abs(computeAngle( [x,y] , [xs, ys], [xf, yf])) <= np.pi/2 and np.abs(computeAngle( [x,y] , [xf, yf], [xs, ys])) <= np.pi/2:
225 |                         v1 = np.array([x,y]) - np.array([xs, ys])
226 |                         angle = computeAngle( [xf,yf] , [xs, ys], [x, y])
227 |                         s_local = la.norm(v1) * np.cos(angle)
228 |                         s       = s_local + PointAndTangent[i, 3]
229 |                         ey      = la.norm(v1) * np.sin(angle)
230 | 
231 |                         if np.abs(ey)<= self.halfWidth + self.slack:
232 |                             CompletedFlag = 1
233 | 
234 |             else:
235 |                 xf = PointAndTangent[i, 0]
236 |                 yf = PointAndTangent[i, 1]
237 |                 xs = PointAndTangent[i - 1, 0]
238 |                 ys = PointAndTangent[i - 1, 1]
239 | 
240 |                 r = 1 / PointAndTangent[i, 5]  # Extract curvature
241 |                 if r >= 0:
242 |                     direction = 1
243 |                 else:
244 |                     direction = -1
245 | 
246 |                 ang = PointAndTangent[i - 1, 2]  # Extract angle of the tangent at the initial point (i-1)
247 | 
248 |                 # Compute the center of the arc
249 |                 CenterX = xs + np.abs(r) * np.cos(ang + direction * np.pi / 2)  # x coordinate center of circle
250 |                 CenterY = ys + np.abs(r) * np.sin(ang + direction * np.pi / 2)  # y coordinate center of circle
251 | 
252 |                 # Check if on the segment using angles
253 |                 if (la.norm(np.array([xs, ys]) - np.array([x, y]))) == 0:
254 |                     ey = 0
255 |                     psi_unwrap = np.unwrap([ang, psi])[1]
256 |                     epsi = psi_unwrap - ang
257 |                     s = PointAndTangent[i, 3]
258 |                     CompletedFlag = 1
259 |                 elif (la.norm(np.array([xf, yf]) - np.array([x, y]))) == 0:
260 |                     s = PointAndTangent[i, 3] + PointAndTangent[i, 4]
261 |                     ey = 0
262 |                     psi_unwrap = np.unwrap([PointAndTangent[i, 2], psi])[1]
263 |                     epsi = psi_unwrap - PointAndTangent[i, 2]
264 |                     CompletedFlag = 1
265 |                 else:
266 |                     arc1 = PointAndTangent[i, 4] * PointAndTangent[i, 5]
267 |                     arc2 = computeAngle([xs, ys], [CenterX, CenterY], [x, y])
268 |                     if np.sign(arc1) == np.sign(arc2) and np.abs(arc1) >= np.abs(arc2):
269 |                         v = np.array([x, y]) - np.array([CenterX, CenterY])
270 |                         s_local = np.abs(arc2)*np.abs(r)
271 |                         s    = s_local + PointAndTangent[i, 3]
272 |                         ey   = -np.sign(direction) * (la.norm(v) - np.abs(r))
273 |                         psi_unwrap = np.unwrap([ang + arc2, psi])[1]
274 |                         epsi = psi_unwrap - (ang + arc2)
275 | 
276 |                         if np.abs(ey) <= self.halfWidth + self.slack:
277 |                             CompletedFlag = 1
278 | 
279 |         if epsi>1.0:
280 |             pdb.set_trace()
281 | 
282 |         if CompletedFlag == 0:
283 |             s    = 10000
284 |             ey   = 10000
285 |             epsi = 10000
286 | 
287 |             print("Error!! POINT OUT OF THE TRACK!!!! <==================")
288 |             pdb.set_trace()
289 | 
290 |         return s, ey, epsi, CompletedFlag
291 | 
292 |     def curvature(self, s):
293 |         """curvature computation
294 |         s: curvilinear abscissa at which the curvature has to be evaluated
295 |         PointAndTangent: points and tangent vectors defining the map (these quantities are initialized in the map object)
296 |         """
297 |         TrackLength = self.PointAndTangent[-1,3]+self.PointAndTangent[-1,4]
298 | 
299 |         # In case on a lap after the first one
300 |         while (s > TrackLength):
301 |             s = s - TrackLength
302 | 
303 |         # Given s \in [0, TrackLength] compute the curvature
304 |         # Compute the segment in which system is evolving
305 |         index = np.all([[s >= self.PointAndTangent[:, 3]], [s < self.PointAndTangent[:, 3] + self.PointAndTangent[:, 4]]], axis=0)
306 | 
307 |         i = int(np.where(np.squeeze(index))[0])
308 |         curvature = self.PointAndTangent[i, 5]
309 | 
310 |         return curvature
311 | 
312 |     def getAngle(self, s, epsi):
313 |         """TO DO
314 |         """
315 |         TrackLength = self.PointAndTangent[-1,3]+self.PointAndTangent[-1,4]
316 | 
317 |         # In case on a lap after the first one
318 |         while (s > TrackLength):
319 |             s = s - TrackLength
320 | 
321 |         # Given s \in [0, TrackLength] compute the curvature
322 |         # Compute the segment in which system is evolving
323 |         index = np.all([[s >= self.PointAndTangent[:, 3]], [s < self.PointAndTangent[:, 3] + self.PointAndTangent[:, 4]]], axis=0)
324 | 
325 |         i = int(np.where(np.squeeze(index))[0])
326 | 
327 |         if i > 0:
328 |             ang = self.PointAndTangent[i - 1, 2]
329 |         else:
330 |             ang = 0
331 | 
332 |         if self.PointAndTangent[i, 5] == 0:
333 |             r= 0
334 |         else:
335 |             r = 1 / self.PointAndTangent[i, 5]  # Radius of curvature
336 | 
337 |         if r == 0:
338 |             # On a straight part of the circuit
339 |             angle_at_s = ang + epsi
340 |         else:
341 |             # On a curve
342 |             cumulative_s = self.PointAndTangent[i, 3]
343 |             relative_s = s - cumulative_s
344 |             spanAng = relative_s / np.abs(r)  # Angle spanned by the circle
345 |             psi = wrap(ang + spanAng * np.sign(r))  # Angle of the tangent vector at the last point of the segment
346 |             # pdb.set_trace()
347 |             angle_at_s = psi + epsi
348 | 
349 |         return angle_at_s
350 | 
351 | # ======================================================================================================================
352 | # ======================================================================================================================
353 | # ====================================== Internal utilities functions ==================================================
354 | # ======================================================================================================================
355 | # ======================================================================================================================
356 | def computeAngle(point1, origin, point2):
357 |     # The orientation of this angle matches that of the coordinate system. Tha is why a minus sign is needed
358 |     v1 = np.array(point1) - np.array(origin)
359 |     v2 = np.array(point2) - np.array(origin)
360 | 
361 |     dot = v1[0] * v2[0] + v1[1] * v2[1]  # dot product between [x1, y1] and [x2, y2]
362 |     det = v1[0] * v2[1] - v1[1] * v2[0]  # determinant
363 |     angle = np.arctan2(det, dot)  # atan2(y, x) or atan2(sin, cos)
364 | 
365 |     return angle # np.arctan2(sinang, cosang)
366 | 
367 | def wrap(angle):
368 |     if angle < -np.pi:
369 |         w_angle = 2 * np.pi + angle
370 |     elif angle > np.pi:
371 |         w_angle = angle - 2 * np.pi
372 |     else:
373 |         w_angle = angle
374 | 
375 |     return w_angle
376 | 
377 | def sign(a):
378 |     if a >= 0:
379 |         res = 1
380 |     else:
381 |         res = -1
382 | 
383 |     return res


--------------------------------------------------------------------------------
/src/initControllerParameters.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | from PredictiveControllers import MPC, LMPC, MPCParams
 3 | 
 4 | def initMPCParams(n, d, N, vt):
 5 |     # Buil the matrices for the state constraint in each region. In the region i we want Fx[i]x <= bx[b]
 6 |     Fx = np.array([[0., 0., 0., 0., 0., 1.],
 7 |                    [0., 0., 0., 0., 0., -1.]])
 8 | 
 9 |     bx = np.array([[2.],   # max ey
10 |                    [2.]]), # max ey
11 | 
12 |     Fu = np.kron(np.eye(2), np.array([1, -1])).T
13 |     bu = np.array([[0.5],   # -Min Steering
14 |                    [0.5],   # Max Steering
15 |                    [10.0],  # -Min Acceleration
16 |                    [10.0]]) # Max Acceleration
17 | 
18 |     # Tuning Parameters
19 |     Q = np.diag([1.0, 1.0, 1, 1, 0.0, 100.0]) # vx, vy, wz, epsi, s, ey
20 |     R = np.diag([1.0, 10.0])                  # delta, a
21 |     xRef   = np.array([vt, 0, 0, 0, 0, 0])
22 |     Qslack = 1 * np.array([0, 50])
23 |     
24 |     mpcParameters    = MPCParams(n=n, d=d, N=N, Q=Q, R=R, Fx=Fx, bx=bx, Fu=Fu, bu=bu, xRef=xRef, slacks=True, Qslack=Qslack)
25 |     mpcParametersLTV = MPCParams(n=n, d=d, N=N, Q=Q, R=R, Fx=Fx, bx=bx, Fu=Fu, bu=bu, xRef=xRef, slacks=True, Qslack=Qslack)       
26 |     return mpcParameters, mpcParametersLTV
27 | 
28 | def initLMPCParams(map, N):
29 |     # Buil the matrices for the state constraint in each region. In the region i we want Fx[i]x <= bx[b]
30 |     Fx = np.array([[0., 0., 0., 0., 0., 1.],
31 |                    [0., 0., 0., 0., 0., -1.]])
32 | 
33 |     bx = np.array([[map.halfWidth],   # max ey
34 |                    [map.halfWidth]]), # max ey
35 | 
36 |     Fu = np.kron(np.eye(2), np.array([1, -1])).T
37 |     bu = np.array([[0.5],   # -Min Steering
38 |                    [0.5],   # Max Steering
39 |                    [10.0],  # -Min Acceleration
40 |                    [10.0]]) # Max Acceleration
41 | 
42 |    # Safe Set Parameters
43 |     numSS_it = 4                  # Number of trajectories used at each iteration to build the safe set
44 |     numSS_Points = 12*numSS_it    # Number of points to select from each trajectory to build the safe set
45 | 
46 |     Laps       = 40+numSS_it      # Total LMPC laps
47 |     TimeLMPC   = 400              # Simulation time
48 | 
49 |     # Tuning Parameters
50 |     QterminalSlack  = 500 * np.diag([1, 1, 1, 1, 1, 1])  # Cost on the slack variable for the terminal constraint
51 |     Qslack  =  1 * np.array([5, 25])                           # Quadratic and linear slack lane cost
52 |     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]
53 |     R_LMPC  =  0 * np.diag([1.0, 1.0])                         # Input cost u = [delta, a]
54 |     dR_LMPC =  5 * np.array([1.0, 10.0])                       # Input rate cost u
55 |     n       = Q_LMPC.shape[0]
56 |     d       = R_LMPC.shape[0]
57 | 
58 |     lmpcParameters    = MPCParams(n=n, d=d, N=N, Q=Q_LMPC, R=R_LMPC, dR=dR_LMPC, Fx=Fx, bx=bx, Fu=Fu, bu=bu, slacks=True, Qslack=Qslack)
59 |     return numSS_it, numSS_Points, Laps, TimeLMPC, QterminalSlack, lmpcParameters


--------------------------------------------------------------------------------
/src/main.py:
--------------------------------------------------------------------------------
  1 | # ----------------------------------------------------------------------------------------------------------------------
  2 | # Licensing Information: You are free to use or extend these projects for
  3 | # education or reserach purposes provided that you provide clear attribution to UC Berkeley,
  4 | # including a reference to the papers describing the control framework:
  5 | # [1] Ugo Rosolia and Francesco Borrelli. "Learning Model Predictive Control for Iterative Tasks. A Data-Driven
  6 | #     Control Framework." In IEEE Transactions on Automatic Control (2017).
  7 | #
  8 | # [2] Ugo Rosolia and Francesco Borrelli "Learning how to autonomously race a car: a predictive control approach" 
  9 | #     In 2017 IEEE Conference on Decision and Control (CDC)
 10 | #
 11 | # [3] Ugo Rosolia and Francesco Borrelli. "Learning Model Predictive Control for Iterative Tasks: A Computationally
 12 | #     IEEE Transactions on Control Systems Technology (2019).
 13 | #
 14 | # Attibution Information: Code developed by Ugo Rosolia
 15 | # (for clarifiactions and suggestions please write to ugo.rosolia@berkeley.edu).
 16 | #
 17 | # Code description: Simulation of the Learning Model Predictive Controller (LMPC). The main file runs:
 18 | # 1) A PID path following controller
 19 | # 2) A Model Predictive Controller (MPC) which uses a LTI model identified from the data collected with the PID in 1)
 20 | # 3) A MPC which uses a LTV model identified from the date collected in 1)
 21 | # 4) A LMPC for racing where the safe set and value function approximation are build using the data from 1), 2) and 3)
 22 | # ----------------------------------------------------------------------------------------------------------------------
 23 | import sys
 24 | sys.path.append('fnc/simulator')
 25 | sys.path.append('fnc/controller')
 26 | sys.path.append('fnc')
 27 | import matplotlib.pyplot as plt
 28 | from plot import plotTrajectory, plotClosedLoopLMPC, animation_xy, animation_states, saveGif_xyResults
 29 | from initControllerParameters import initMPCParams, initLMPCParams
 30 | from PredictiveControllers import MPC, LMPC, MPCParams
 31 | from PredictiveModel import PredictiveModel
 32 | from Utilities import Regression, PID
 33 | from SysModel import Simulator
 34 | from Track import Map
 35 | import numpy as np
 36 | import pickle
 37 | import pdb
 38 | 
 39 | def main():
 40 |     # ======================================================================================================================
 41 |     # ============================================= Initialize parameters  =================================================
 42 |     # ======================================================================================================================
 43 |     N = 14                                    # Horizon length
 44 |     n = 6;   d = 2                            # State and Input dimension
 45 |     x0 = np.array([0.5, 0, 0, 0, 0, 0])       # Initial condition
 46 |     xS = [x0, x0]
 47 |     dt = 0.1
 48 | 
 49 |     map = Map(0.4)                            # Initialize map
 50 |     vt = 0.8                                  # target vevlocity
 51 | 
 52 |     # Initialize controller parameters
 53 |     mpcParam, ltvmpcParam = initMPCParams(n, d, N, vt)
 54 |     numSS_it, numSS_Points, Laps, TimeLMPC, QterminalSlack, lmpcParameters = initLMPCParams(map, N)
 55 | 
 56 |     # Init simulators
 57 |     simulator     = Simulator(map)
 58 |     LMPCsimulator = Simulator(map, multiLap = False, flagLMPC = True)
 59 | 
 60 |     # ======================================================================================================================
 61 |     # ======================================= PID path following ===========================================================
 62 |     # ======================================================================================================================
 63 |     print("Starting PID")
 64 |     # Initialize pid and run sim
 65 |     PIDController = PID(vt)
 66 |     xPID_cl, uPID_cl, xPID_cl_glob, _ = simulator.sim(xS, PIDController)
 67 |     print("===== PID terminated")
 68 | 
 69 |     # ======================================================================================================================
 70 |     # ======================================  LINEAR REGRESSION ============================================================
 71 |     # ======================================================================================================================
 72 |     print("Starting MPC")
 73 |     # Estimate system dynamics
 74 |     lamb = 0.0000001
 75 |     A, B, Error = Regression(xPID_cl, uPID_cl, lamb)
 76 |     mpcParam.A = A
 77 |     mpcParam.B = B
 78 |     # Initialize MPC and run closed-loop sim
 79 |     mpc = MPC(mpcParam)
 80 |     xMPC_cl, uMPC_cl, xMPC_cl_glob, _ = simulator.sim(xS, mpc)
 81 |     print("===== MPC terminated")
 82 | 
 83 |     # ======================================================================================================================
 84 |     # ===================================  LOCAL LINEAR REGRESSION =========================================================
 85 |     # ======================================================================================================================
 86 |     print("Starting TV-MPC")
 87 |     # Initialized predictive model
 88 |     predictiveModel = PredictiveModel(n, d, map, 1)
 89 |     predictiveModel.addTrajectory(xPID_cl,uPID_cl)
 90 |     #Initialize TV-MPC
 91 |     ltvmpcParam.timeVarying = True 
 92 |     mpc = MPC(ltvmpcParam, predictiveModel)
 93 |     # Run closed-loop sim
 94 |     xTVMPC_cl, uTVMPC_cl, xTVMPC_cl_glob, _ = simulator.sim(xS, mpc)
 95 |     print("===== TV-MPC terminated")
 96 | 
 97 |     # ======================================================================================================================
 98 |     # ==============================  LMPC w\ LOCAL LINEAR REGRESSION ======================================================
 99 |     # ======================================================================================================================
100 |     print("Starting LMPC")
101 |     # Initialize Predictive Model for lmpc
102 |     lmpcpredictiveModel = PredictiveModel(n, d, map, 4)
103 |     for i in range(0,4): # add trajectories used for model learning
104 |         lmpcpredictiveModel.addTrajectory(xPID_cl,uPID_cl)
105 | 
106 |     # Initialize Controller
107 |     lmpcParameters.timeVarying     = True 
108 |     lmpc = LMPC(numSS_Points, numSS_it, QterminalSlack, lmpcParameters, lmpcpredictiveModel)
109 |     for i in range(0,4): # add trajectories for safe set
110 |         lmpc.addTrajectory( xPID_cl, uPID_cl, xPID_cl_glob)
111 |     
112 |     # Run sevaral laps
113 |     for it in range(numSS_it, Laps):
114 |         # Simulate controller
115 |         xLMPC, uLMPC, xLMPC_glob, xS = LMPCsimulator.sim(xS,  lmpc)
116 |         # Add trajectory to controller
117 |         lmpc.addTrajectory( xLMPC, uLMPC, xLMPC_glob)
118 |         # lmpcpredictiveModel.addTrajectory(np.append(xLMPC,np.array([xS[0]]),0),np.append(uLMPC, np.zeros((1,2)),0))
119 |         lmpcpredictiveModel.addTrajectory(xLMPC,uLMPC)
120 |         print("Completed lap: ", it, " in ", np.round(lmpc.Qfun[it][0]*dt, 2)," seconds")
121 |     print("===== LMPC terminated")
122 | 
123 |     # # ======================================================================================================================
124 |     # # ========================================= PLOT TRACK =================================================================
125 |     # # ======================================================================================================================
126 |     for i in range(0, lmpc.it):
127 |         print("Lap time at iteration ", i, " is ",np.round( lmpc.Qfun[i][0]*dt, 2), "s")
128 | 
129 |     print("===== Start Plotting")
130 |     plotTrajectory(map, xPID_cl, xPID_cl_glob, uPID_cl, 'PID')
131 |     plotTrajectory(map, xMPC_cl, xMPC_cl_glob, uMPC_cl, 'MPC')
132 |     plotTrajectory(map, xTVMPC_cl, xTVMPC_cl_glob, uTVMPC_cl, 'TV-MPC')
133 |     plotClosedLoopLMPC(lmpc, map)
134 |     animation_xy(map, lmpc, Laps-1)
135 |     plt.show()
136 | 
137 |     # animation_states(map, LMPCOpenLoopData, lmpc, Laps-2)
138 |     # animation_states(map, LMPCOpenLoopData, lmpc, Laps-2)
139 |     # animation_states(map, LMPCOpenLoopData, lmpc, Laps-2)
140 |     # animation_states(map, LMPCOpenLoopData, lmpc, Laps-2)
141 |     # saveGif_xyResults(map, LMPCOpenLoopData, lmpc, Laps-2)
142 | 
143 | if __name__== "__main__":
144 |   main()


--------------------------------------------------------------------------------