├── .gitignore ├── Nominal_LMPC_Chapter ├── Ex1_LMPC_for_double_integrator │ ├── Ex1_LMPC_for_double_integrator.ipynb │ ├── LinearLMPC_RoA_comparison.pdf │ ├── LinearLMPC_cost.pdf │ └── fnc │ │ ├── FTO.py │ │ └── LMPC_CS.py ├── Ex2_LMPC_for_minimum_time_unicycle │ ├── Ex2_LMPC_for_minimum_time_unicycle.ipynb │ ├── NonlinearLMPC_Cost.pdf │ ├── NonlinearLMPC_Task.pdf │ └── fnc │ │ ├── FTO.py │ │ ├── LMPC_CT.py │ │ └── MIX_OL_CL.py └── Ex3_LMPC_for_autonomous_racing │ ├── Ex3_LMPC_for_autonomous_racing.ipynb │ ├── closedloop.pdf │ ├── fnc │ ├── PredictiveControllers.py │ ├── PredictiveModel.py │ ├── Track.py │ ├── Utilities.py │ └── plot.py │ ├── initControllerParameters.py │ └── predicted.pdf ├── README.md ├── Stochastic_Safe_Sets_Chapter └── Ex1_Sample_based_safe_set │ ├── Ex1_Sample_based_safe_set.ipynb │ ├── fnc │ ├── build_control_invariant.py │ ├── build_robust_invariant.py │ └── mpc.py │ └── invariant.pdf ├── py3.yml └── utils ├── .DS_Store ├── .gitignore ├── polytope.py ├── simulators.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .DS* 3 | *.ipynb_checkpoints 4 | *.png -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex1_LMPC_for_double_integrator/LinearLMPC_RoA_comparison.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/Nominal_LMPC_Chapter/Ex1_LMPC_for_double_integrator/LinearLMPC_RoA_comparison.pdf -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex1_LMPC_for_double_integrator/LinearLMPC_cost.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/Nominal_LMPC_Chapter/Ex1_LMPC_for_double_integrator/LinearLMPC_cost.pdf -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex1_LMPC_for_double_integrator/fnc/FTO.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pdb 3 | import scipy 4 | from cvxpy import * 5 | 6 | class FTOCP(object): 7 | """ Finite Time Optimal Control Problem (FTOCP) 8 | Methods: 9 | - solve: solves the FTOCP given the initial condition x0, terminal contraints (optinal) and terminal cost (optional) 10 | - model: given x_t and u_t computes x_{t+1} = Ax_t + Bu_t 11 | 12 | """ 13 | def __init__(self, N, A, B, Q1, Q, R, x_max, u_max): 14 | # Define variables 15 | self.N = N # Horizon Length 16 | 17 | # System Dynamics (x_{k+1} = A x_k + Bu_k) 18 | self.A = A 19 | self.B = B 20 | self.n = A.shape[1] 21 | self.d = B.shape[1] 22 | self.x_max = x_max 23 | self.u_max = u_max 24 | 25 | # Cost (h(x,u) = x^TQx +u^TRu) 26 | self.Q = Q 27 | self.Q1 = Q1 28 | self.R = R 29 | 30 | # Initialize Predicted Trajectory 31 | self.xPred = [] 32 | self.uPred = [] 33 | 34 | def solve(self, x0, verbose=False): 35 | """This method solves an FTOCP given: 36 | - x0: initial condition 37 | """ 38 | 39 | # Initialize Variables 40 | x = Variable((self.n, self.N+1)) 41 | u = Variable((self.d, self.N)) 42 | 43 | # State Constraints 44 | constr = [x[:,0] == x0[:]] 45 | for i in range(0, self.N): 46 | constr += [x[:,i+1] == self.A@x[:,i] + self.B@u[:,i], 47 | u[:,i] >= -self.u_max, 48 | u[:,i] <= self.u_max, 49 | x[:,i] >= -self.x_max, 50 | x[:,i] <= self.x_max] 51 | 52 | # Cost Function 53 | cost = 0 54 | for i in range(0, self.N): 55 | # Running cost h(x,u) = x^TQx + u^TRu 56 | cost += quad_form(x[:,i], self.Q) + quad_form(u[:,i], self.R) + norm(self.Q1@x[:,i], 1) 57 | # cost += norm(self.Q**0.5*x[:,i])**2 + norm(self.R**0.5*u[:,i])**2 58 | 59 | 60 | # Solve the Finite Time Optimal Control Problem 61 | problem = Problem(Minimize(cost), constr) 62 | problem.solve(verbose=verbose, solver=ECOS) 63 | 64 | if problem.status != "optimal": 65 | print("problem.status: ", problem.status) 66 | 67 | # Store the open-loop predicted trajectory 68 | self.xPred = x.value 69 | self.uPred = u.value 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex1_LMPC_for_double_integrator/fnc/LMPC_CS.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from numpy import linalg as la 4 | import pdb 5 | import copy 6 | import itertools 7 | import numpy as np 8 | import pdb 9 | import scipy 10 | from cvxpy import * 11 | 12 | class LMPC_CS(object): 13 | """Learning Model Predictive Controller (LMPC) implemented using the Convex Safe set (CS) 14 | Inputs: 15 | - A,B: System dyamics 16 | - x_max, u_max: max of the infinity norm constraint 17 | - Q1, Q, R: cost matrices 18 | Methods: 19 | - addTrajectory: adds a trajectory to the safe set SS and update value function 20 | - computeCost: computes the cost associated with a feasible trajectory 21 | - solve: uses ftocp and the stored data to comptute the predicted trajectory""" 22 | def __init__(self, N, A, B, Q1, Q, R, x_max, u_max, verbose = False): 23 | # Initialization 24 | self.N = N # Horizon Length 25 | self.it_cost = [] 26 | self.Q1 = Q1 27 | self.Q = Q 28 | self.R = R 29 | self.A = A 30 | self.B = B 31 | self.x_max = x_max 32 | self.u_max = u_max 33 | self.n = A.shape[1] 34 | self.d = B.shape[1] 35 | self.SS = np.zeros((self.n,1)) 36 | self.uSS = np.zeros((self.d,1)) 37 | self.Vfun = np.zeros(1) 38 | self.verbose = False 39 | 40 | def addTrajectory(self, x, u): 41 | # Add the feasible trajectory x and the associated input sequence u to the safe set 42 | self.SS = np.hstack((self.SS, np.array(x).T)) 43 | self.uSS = np.hstack((self.uSS, np.array(u).T.reshape(self.d,-1))) 44 | 45 | # Compute and store the cost associated with the feasible trajectory 46 | cost = self.computeCost(x, u) 47 | self.Vfun = np.append(self.Vfun, np.array(cost)) 48 | 49 | # Augment iteration counter and print the cost of the trajectories stored in the safe set 50 | self.it_cost.append(cost[0]) 51 | if self.verbose == True: 52 | print("Trajectory added to the Safe Set. Current Iteration: ", len(self.it_cost)) 53 | print("Performance stored trajectories: \n", [cost for cost in self.it_cost]) 54 | 55 | def computeCost(self, x, u): 56 | # Compute the cost in a DP like strategy: start from the last point x[len(x)-1] and move backwards 57 | for i in range(0,len(x)): 58 | idx = len(x)-1 - i 59 | if i == 0: 60 | cost = [np.dot(np.dot(x[idx],self.Q),x[idx]) + np.linalg.norm(self.Q1@x[idx], 1)] 61 | else: 62 | cost.append(float(np.dot(np.dot(x[idx],self.Q),x[idx]) + np.linalg.norm(self.Q1@x[idx], 1) + np.dot(np.dot(u[idx],self.R),u[idx]) + cost[-1])) 63 | 64 | # Finally flip the cost to have correct order 65 | return np.flip(cost).tolist() 66 | 67 | def solve(self, x0, verbose = False): 68 | """This method solves an FTOCP given: 69 | - x0: initial condition 70 | - SS: (optional) contains a set of state and the terminal constraint is ConvHull(SS) 71 | - Vfun: (optional) cost associtated with the state stored in SS. Terminal cost is BarycentrcInterpolation(SS, Vfun) 72 | """ 73 | 74 | # Initialize Variables 75 | x = Variable((self.n, self.N+1)) 76 | u = Variable((self.d, self.N)) 77 | lambVar = Variable((self.SS.shape[1], 1)) # Initialize vector of variables 78 | 79 | # State Constraints 80 | constr = [x[:,0] == x0[:]] 81 | for i in range(0, self.N): 82 | constr += [x[:,i+1] == self.A @ x[:,i] + self.B @ u[:,i], 83 | u[:,i] >= -self.u_max, 84 | u[:,i] <= self.u_max, 85 | x[:,i] >= -self.x_max, 86 | x[:,i] <= self.x_max] 87 | 88 | # Enforce terminal state into the convex safe set 89 | constr += [self.SS @ lambVar[:,0] == x[:,self.N], # Terminal state \in ConvHull(SS) 90 | np.ones((1, self.SS.shape[1])) @ lambVar[:,0] == 1, # Multiplies \lambda sum to 1 91 | lambVar >= 0] # Multiplier are positive definite 92 | 93 | # Cost Function 94 | cost = 0 95 | for i in range(0, self.N): 96 | # Running cost h(x,u) = x^TQx + u^TRu 97 | cost += quad_form(x[:,i], self.Q) + norm(self.Q1@x[:,i], 1) + quad_form(u[:,i], self.R) 98 | 99 | # Terminal cost if SS not empty 100 | cost += self.Vfun @ lambVar[:,0] # It terminal cost is given by interpolation using \lambda 101 | 102 | # Solve the Finite Time Optimal Control Problem 103 | problem = Problem(Minimize(cost), constr) 104 | problem.solve(verbose=verbose, solver=ECOS) 105 | if problem.status != "optimal": 106 | print("problem.status: ", problem.status) 107 | 108 | # Store the open-loop predicted trajectory 109 | self.xPred = x.value 110 | self.uPred = u.value 111 | # self.lamb = lambVar.value 112 | -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex2_LMPC_for_minimum_time_unicycle/NonlinearLMPC_Cost.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/Nominal_LMPC_Chapter/Ex2_LMPC_for_minimum_time_unicycle/NonlinearLMPC_Cost.pdf -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex2_LMPC_for_minimum_time_unicycle/NonlinearLMPC_Task.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/Nominal_LMPC_Chapter/Ex2_LMPC_for_minimum_time_unicycle/NonlinearLMPC_Task.pdf -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex2_LMPC_for_minimum_time_unicycle/fnc/FTO.py: -------------------------------------------------------------------------------- 1 | from casadi import * 2 | from numpy import * 3 | import pdb 4 | import itertools 5 | import numpy as np 6 | from cvxpy import * 7 | 8 | ##### MY CODE ###### 9 | class FTO(object): 10 | """ Finite Time Optimal Control Problem (FTO) 11 | Methods: 12 | - solve: solves the FTO given the initial condition x0 and terminal contraints 13 | - buildNonlinearProgram: builds the nonlinear program solved by the above solve methos 14 | - model: given x_t and u_t computes x_{t+1} = Ax_t + Bu_t 15 | 16 | """ 17 | def __init__(self, N, roadHalfWidth): 18 | # Define variables 19 | self.N = N 20 | self.n = 3 21 | self.d = 2 22 | self.radius = 10.0 23 | self.optCost= np.inf 24 | self.dt = 0.5#0.25 25 | self.dimSS = [] 26 | self.roadHalfWidth = roadHalfWidth 27 | 28 | def set_xf(self, xf): 29 | # Set terminal state 30 | if xf.shape[1] >1: 31 | self.terminalSet = True 32 | self.xf_lb = xf[:,0] 33 | self.xf_ub = xf[:,1] 34 | else: 35 | self.terminalSet = False 36 | self.xf = xf[:,0] 37 | self.xf_lb = self.xf 38 | self.xf_ub = self.xf 39 | 40 | def checkTaskCompletion(self,x): 41 | # Check if the task was completed 42 | taskCompletion = False 43 | if (self.terminalSet == True) and (self.xf_lb <= x).all() and (x <= self.xf_ub).all(): 44 | taskCompletion = True 45 | elif (self.terminalSet == False) and np.dot(x-self.xf, x-self.xf)<= 1e-8: 46 | taskCompletion = True 47 | 48 | return taskCompletion 49 | 50 | 51 | def solve(self, x0, zt): 52 | # Initialize initial guess for lambda 53 | lambGuess = np.concatenate((np.ones(self.dimSS)/self.dimSS, np.zeros(self.n)), axis = 0) 54 | lambGuess[0] = 1 55 | self.xGuessTot = np.concatenate( (self.xGuess, lambGuess), axis=0 ) 56 | 57 | # Need to solve N+1 ftocp as the stage cost is the indicator function --> try all configuration 58 | costSolved = [] 59 | soluSolved = [] 60 | slackNorm = [] 61 | for i in range(0, self.N+1): 62 | # IMPORTANT: here 'i' represents the number of states constrained to the safe set --> the horizon length is (N-i) 63 | if i is not self.N: 64 | # Set box constraints on states (here we constraint the last i steps of the horizon to be xf) 65 | self.lbx = x0 + [-100, -self.roadHalfWidth, -0]*(self.N-i)+ self.xf_lb.tolist()*i + [-2.0,-1.0]*self.N + [0]*self.dimSS + [-10]*self.n 66 | self.ubx = x0 + [100, self.roadHalfWidth, 500]*(self.N-i)+ self.xf_ub.tolist()*i + [2.0, 1.0]*self.N + [10]*self.dimSS + [10]*self.n 67 | 68 | # Solve nonlinear programm 69 | sol = self.solver(lbx=self.lbx, ubx=self.ubx, lbg=self.lbg_dyanmics, ubg=self.ubg_dyanmics, x0 = self.xGuessTot.tolist()) 70 | 71 | # Check if the solution is feasible 72 | idxSlack = (self.N+1)*self.n + self.d*self.N + self.dimSS 73 | self.slack = sol["x"][idxSlack:idxSlack+self.n] 74 | slackNorm.append(np.linalg.norm(self.slack,2)) 75 | if (self.solver.stats()['success']) and (np.linalg.norm(self.slack,2)< 1e-8): 76 | self.feasible = 1 77 | # Notice that the cost is given by the cost of the ftocp + the number of steps not constrainted to be xf 78 | lamb = sol["x"][((self.N+1)*self.n+self.N*self.d):((self.N+1)*self.n + self.d*self.N + self.dimSS)] 79 | terminalCost = np.dot(self.costSS, lamb) 80 | # costSolved.append(terminalCost+(self.N+1-i)) 81 | if i == 0: 82 | costSolved.append(terminalCost+(self.N-i)) 83 | else: 84 | costSolved.append(terminalCost+(self.N-(i-1))) 85 | 86 | soluSolved.append(sol) 87 | else: 88 | costSolved.append(np.inf) 89 | soluSolved.append(sol) 90 | self.feasible = 0 91 | 92 | else: # if horizon one time step (because N-i = 0) --> just check feasibility of the initial guess 93 | uGuess = self.xGuess[(self.n*(self.N+1)):(self.n*(self.N+1)+self.d)] 94 | xNext = self.f(x0, uGuess) 95 | slackNorm.append(0.0) 96 | if self.checkTaskCompletion(xNext): 97 | self.feasible = 1 98 | costSolved.append(1) 99 | sol["x"] = self.xGuessTot 100 | soluSolved.append(sol) 101 | else: 102 | costSolved.append(np.inf) 103 | soluSolved.append(sol) 104 | self.feasible = 0 105 | 106 | 107 | # Check if LMPC cost is decreasing (it should as it is a Lyap function) 108 | if np.min(costSolved) > self.optCost: 109 | print "Cost not decreasing: ", self.optCost, np.min(costSolved) 110 | pdb.set_trace() 111 | 112 | 113 | # Store optimal solution 114 | self.optCost = np.min(costSolved) 115 | x = np.array(soluSolved[np.argmin(costSolved)]["x"]) 116 | self.xSol = x[0:(self.N+1)*self.n].reshape((self.N+1,self.n)).T 117 | self.uSol = x[(self.N+1)*self.n:((self.N+1)*self.n + self.d*self.N)].reshape((self.N,self.d)).T 118 | self.lamb = x[((self.N+1)*self.n+self.N*self.d):((self.N+1)*self.n + self.d*self.N + self.dimSS)] 119 | optSlack = slackNorm[np.argmin(costSolved)] 120 | 121 | 122 | if self.verbose == True: 123 | print "Slack Norm: ", optSlack 124 | print "Cost Vector:", costSolved, np.argmin(costSolved) 125 | print "Optimal Solution:", self.xSol 126 | 127 | def buildNonlinearProgram(self, SSQfun): 128 | # Define variables 129 | n = self.n 130 | d = self.d 131 | N = self.N 132 | X = SX.sym('X', n*(N+1)); 133 | U = SX.sym('X', d*N); 134 | dimSS = np.shape(SSQfun)[1] 135 | lamb = SX.sym('X', dimSS) 136 | xSS = SSQfun[0:n, :] 137 | costSS = np.array([SSQfun[-1, :]]) 138 | slack = SX.sym('X', n); 139 | 140 | self.dimSS = dimSS 141 | self.SSQfun = SSQfun 142 | self.xSS = xSS 143 | self.costSS = costSS 144 | self.xSS = xSS 145 | self.costSS = costSS 146 | 147 | # Define dynamic constraints 148 | constraint = [] 149 | for i in range(0, N): 150 | constraint = vertcat(constraint, X[n*(i+1)+0] - (X[n*i+0] + self.dt*X[n*i+2]*np.cos( U[d*i+0] - X[n*i+0] / self.radius) / (1 - X[n*i+1]/self.radius ) )) 151 | constraint = vertcat(constraint, X[n*(i+1)+1] - (X[n*i+1] + self.dt*X[n*i+2]*np.sin( U[d*i+0] - X[n*i+0] / self.radius) )) 152 | constraint = vertcat(constraint, X[n*(i+1)+2] - (X[n*i+2] + self.dt*U[d*i+1])) 153 | 154 | # terminal constraints 155 | constraint = vertcat(constraint, slack + X[n*N:(n*(N+1))+0] - mtimes( xSS ,lamb) ) 156 | constraint = vertcat(constraint, 1 - mtimes(np.ones((1, dimSS )), lamb ) ) 157 | 158 | # Defining Cost (We will add stage cost later) 159 | cost = mtimes(costSS, lamb) + 1000000**2*(slack[0]**2 + slack[1]**2 + slack[2]**2) 160 | 161 | # Set IPOPT options 162 | # opts = {"verbose":False,"ipopt.print_level":0,"print_time":0,"ipopt.mu_strategy":"adaptive","ipopt.mu_init":1e-5,"ipopt.mu_min":1e-15,"ipopt.barrier_tol_factor":1}#, "ipopt.acceptable_constr_viol_tol":0.001}#,"ipopt.acceptable_tol":1e-4}#, "expand":True} 163 | opts = {"verbose":False,"ipopt.print_level":0,"print_time":0}#, "ipopt.acceptable_constr_viol_tol":0.001}#,"ipopt.acceptable_tol":1e-4}#, "expand":True} 164 | nlp = {'x':vertcat(X,U,lamb,slack), 'f':cost, 'g':constraint} 165 | self.solver = nlpsol('solver', 'ipopt', nlp, opts) 166 | 167 | # Set lower bound of inequality constraint to zero to force: 1) n*N state dynamics, 2) n terminal contraints and 3) CVX hull constraint 168 | self.lbg_dyanmics = [0]*(n*N) + [0]*(n) + [0] 169 | self.ubg_dyanmics = [0]*(n*N) + [0]*(n) + [0] 170 | 171 | def f(self, x, u): 172 | # Given a state x and input u it return the successor state 173 | xNext = np.array([x[0] + self.dt * x[2]*np.cos(u[0] - x[0]/self.radius) / (1 - x[1] / self.radius), 174 | x[1] + self.dt * x[2]*np.sin(u[0] - x[0]/self.radius), 175 | x[2] + self.dt * u[1]]) 176 | return xNext.tolist() 177 | -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex2_LMPC_for_minimum_time_unicycle/fnc/LMPC_CT.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from numpy import linalg as la 4 | import pdb 5 | import copy 6 | import itertools 7 | from casadi import * 8 | from numpy import * 9 | import pdb 10 | import itertools 11 | import numpy as np 12 | from cvxpy import * 13 | 14 | class LMPC_CT(object): 15 | """Learning Model Predictive Controller (LMPC) implemented using the Convex Time varing safe set (CT) 16 | Inputs: 17 | - ftocp: Finite Time Optimal Control Prolem object used to compute the predicted trajectory 18 | - l: number of past trajectories used to construct the local safe set and local Q-function 19 | - M: number of data points from each trajectory used to construct the local safe set and local Q-function 20 | Methods: 21 | - addTrajectory: adds a trajectory to the safe set SS and update value function 22 | - computeCost: computes the cost associated with a feasible trajectory 23 | - solve: uses ftocp and the stored data to comptute the predicted trajectory 24 | - closeToSS: computes the K-nearest neighbors to zt""" 25 | 26 | def __init__(self, N, dt, roadHalfWidth, l, P, Xf_vertices = None, verbose = False): 27 | self.N = N 28 | self.n = 3 29 | self.d = 2 30 | self.radius = 10.0 31 | self.optCost= np.inf 32 | self.dt = dt 33 | self.dimSS = [] 34 | self.roadHalfWidth = roadHalfWidth 35 | 36 | self.SS = [] 37 | self.uSS = [] 38 | self.Vfun = [] 39 | self.l = l 40 | self.P = P 41 | self.zt = [] 42 | self.it = 0 43 | self.timeVarying = True 44 | self.itCost = [] 45 | self.verbose = verbose 46 | self.Xf_vertices = Xf_vertices 47 | 48 | def addTrajectory(self, x, u): 49 | # Add the feasible trajectory x and the associated input sequence u to the safe set 50 | self.SS.append(np.array(x).T) 51 | self.uSS.append(np.concatenate( (np.array(u).T, np.zeros((np.array(u).T.shape[0],1)) ), axis=1)) # Here concatenating zero as f(xf, 0) = xf by assumption 52 | 53 | # Compute and store the cost associated with the feasible trajectory 54 | self.Vfun.append(np.arange(np.array(x).T.shape[1]-1,-1,-1)) 55 | 56 | # Compute initial guess for nonlinear solver and store few variables 57 | self.xGuess = np.concatenate((np.array(x).T[:,0:(self.N+1)].T.flatten(), np.array(u).T[:,0:(self.N)].T.flatten()), axis = 0) 58 | 59 | # Initialize cost varaibles for bookkeeping 60 | self.cost = self.Vfun[-1][0] 61 | self.itCost.append(self.cost) 62 | self.optCost = self.cost + 1 63 | self.oldIt = self.it 64 | 65 | # Pass inital guess to ftopc object 66 | self.xSol = np.array(x).T[:,0:(self.N+1)] 67 | self.uSol = np.array(u).T.reshape(self.d,-1)[:,0:(self.N)] 68 | 69 | # Update time Improvement counter 70 | self.timeImprovement = 0 71 | 72 | # Update iteration counter 73 | self.it = self.it + 1 74 | 75 | # Update indices of stored data points used to contruct the local safe set and Q-function 76 | self.SSindices =[] 77 | Tstar = np.min(self.itCost) 78 | for i in range(0, self.it): 79 | Tj = np.shape(self.SS[i])[1]-1 80 | self.SSindices.append(np.arange(Tj - Tstar + self.N, Tj - Tstar + self.N+self.P)) 81 | 82 | def solve(self, xt, verbose = False): 83 | 84 | # First retive the data points used to cconstruct the safe set. 85 | minIt = np.max([0, self.it - self.l]) 86 | SSVfun = [] 87 | SSnext = [] 88 | # Loop over j-l iterations used to contruct the local safe set 89 | for i in range(minIt, self.it): 90 | # idx associated with the data points from iteration i which are in the local safe set 91 | idx = self.timeSS(i) 92 | # Stored state and cost value (Check if using terminal state or terminal point) 93 | if (self.Xf_vertices is not None) and (idx[-1] == np.shape(self.SS[i])[1]-1): 94 | augSS = np.concatenate((self.SS[i][:,idx], self.Xf_vertices), axis=1 ) 95 | augCost = np.concatenate((self.Vfun[i][idx], np.zeros(self.Xf_vertices.shape[1])), axis=0 ) 96 | SSVfun.append( np.concatenate( (augSS, [augCost]), axis=0 ).T ) 97 | 98 | # Store the successors of the states into the safe set and the control action. (Note that the vertices of X_f are invariant states) 99 | # This matrix will be used to compute the vector zt which represent a feasible guess for the ftocp at time t+1 100 | xSSuSS = np.concatenate((self.SS[i], self.uSS[i]), axis = 0) 101 | extendedSS = np.concatenate((xSSuSS, np.array([xSSuSS[:,-1]]).T), axis=1) 102 | verticesAndInputs = np.concatenate((self.Xf_vertices, np.zeros((self.d, self.Xf_vertices.shape[1]))), axis=0) 103 | SSnext.append(np.concatenate((extendedSS[:,idx+1], verticesAndInputs), axis = 1).T) 104 | else: 105 | SSVfun.append( np.concatenate( (self.SS[i][:,idx], [self.Vfun[i][idx]]), axis=0 ).T ) 106 | # Store the successors of the states into the safe set and the control action. 107 | # This matrix will be used to compute a feasible guess for the ftocp at time t+1 108 | xSSuSS = np.concatenate((self.SS[i], self.uSS[i]), axis = 0) 109 | extendedSS = np.concatenate((xSSuSS, np.array([xSSuSS[:,-1]]).T), axis=1) 110 | SSnext.append(extendedSS[:,idx+1].T) 111 | 112 | 113 | # From a 3D list to a 2D array 114 | SSVfun_vector = np.squeeze(list(itertools.chain.from_iterable(SSVfun))).T 115 | SSnext_vector = np.squeeze(list(itertools.chain.from_iterable(SSnext))).T 116 | 117 | # Add dimension if needed 118 | if SSVfun_vector.ndim == 1: 119 | SSVfun_vector = np.array([SSVfun_vector]).T 120 | 121 | # Now update ftocp with local safe set 122 | self.buildNonlinearProgram( SSVfun_vector) 123 | 124 | # Now solve ftocp 125 | self.solve_ftocp(xt) 126 | 127 | # Assign input 128 | self.uPred = self.uSol 129 | 130 | # Update guess for the ftocp using optimal predicted trajectory and multipliers lambda 131 | if self.optCost > 1: 132 | xfufNext = np.dot(SSnext_vector, self.lamb) 133 | # Update initial guess 134 | xflatOpenLoop = np.concatenate( (self.xSol[:,1:(self.N+1)].T.flatten(), xfufNext[0:self.n,0]), axis = 0) 135 | uflatOpenLoop = np.concatenate( (self.uSol[:,1:(self.N)].T.flatten() , xfufNext[self.n:(self.n+self.d),0]), axis = 0) 136 | self.xGuess = np.concatenate((xflatOpenLoop, uflatOpenLoop) , axis = 0) 137 | 138 | def timeSS(self, it): 139 | # This function computes the indices used to construct the safe set 140 | # self.SSindices[it] is initialized when the trajectory is added to the safe set after computing \delta^i and P 141 | 142 | # Read the time indices 143 | currIdx = self.SSindices[it] 144 | # By definition we have x_t^j = x_F \forall t > T^j ---> check indices to select 145 | # currIdxShort = currIdx[ (currIdx >0) & (currIdx < np.shape(self.SS[it])[1])] 146 | currIdxShort = currIdx[ currIdx < np.shape(self.SS[it])[1] ] 147 | 148 | # Progress time indices 149 | self.SSindices[it] = self.SSindices[it] + 1 150 | 151 | # If there is just one time index --> add dimension 152 | if np.shape(currIdxShort)[0] < 1: 153 | currIdxShort = np.array([np.shape(self.SS[it])[1]-1]) 154 | 155 | return currIdxShort 156 | 157 | 158 | def set_xf(self, xf): 159 | # Set terminal state 160 | if xf.shape[1] >1: 161 | self.terminalSet = True 162 | self.xf_lb = xf[:,0] 163 | self.xf_ub = xf[:,1] 164 | else: 165 | self.terminalSet = False 166 | self.xf = xf[:,0] 167 | self.xf_lb = self.xf 168 | self.xf_ub = self.xf 169 | 170 | def checkTaskCompletion(self,x): 171 | # Check if the task was completed 172 | taskCompletion = False 173 | if (self.terminalSet == True) and (self.xf_lb <= x).all() and (x <= self.xf_ub).all(): 174 | taskCompletion = True 175 | elif (self.terminalSet == False) and np.dot(x-self.xf, x-self.xf)<= 1e-8: 176 | taskCompletion = True 177 | 178 | return taskCompletion 179 | 180 | 181 | def solve_ftocp(self, x0, verbose=False): 182 | # Initialize initial guess for lambda 183 | lambGuess = np.hstack( (np.ones(self.dimSS)/self.dimSS, np.zeros(self.n)) ) 184 | lambGuess[0] = 1 185 | self.xGuessTot = np.hstack( (self.xGuess, lambGuess) ) 186 | 187 | # lambGuess = np.concatenate((np.ones(self.dimSS)/self.dimSS, np.zeros(self.n)), axis = 0) 188 | # lambGuess[0] = 1 189 | # self.xGuessTot = np.concatenate( (self.xGuess, lambGuess), axis=0 ) 190 | 191 | # Need to solve N+1 ftocp as the stage cost is the indicator function --> try all configuration 192 | costSolved = [] 193 | soluSolved = [] 194 | slackNorm = [] 195 | for i in range(0, self.N+1): 196 | # IMPORTANT: here 'i' represents the number of states constrained to the safe set --> the horizon length is (N-i) 197 | if i is not self.N: 198 | # Set box constraints on states (here we constraint the last i steps of the horizon to be xf) 199 | self.lbx = x0 + [-100, -self.roadHalfWidth, -0]*(self.N-i)+ self.xf_lb.tolist()*i + [-2.0,-1.0]*self.N + [0]*self.dimSS + [-10]*self.n 200 | self.ubx = x0 + [100, self.roadHalfWidth, 500]*(self.N-i)+ self.xf_ub.tolist()*i + [2.0, 1.0]*self.N + [10]*self.dimSS + [10]*self.n 201 | 202 | # Solve nonlinear programm 203 | sol = self.solver(lbx=self.lbx, ubx=self.ubx, lbg=self.lbg_dyanmics, ubg=self.ubg_dyanmics, x0 = self.xGuessTot.tolist()) 204 | 205 | # Check if the solution is feasible 206 | idxSlack = (self.N+1)*self.n + self.d*self.N + self.dimSS 207 | self.slack = sol["x"][idxSlack:idxSlack+self.n] 208 | slackNorm.append(np.linalg.norm(self.slack,2)) 209 | if (self.solver.stats()['success']) and (np.linalg.norm(self.slack,2)< 1e-8): 210 | self.feasible = 1 211 | # Notice that the cost is given by the cost of the ftocp + the number of steps not constrainted to be xf 212 | lamb = sol["x"][((self.N+1)*self.n+self.N*self.d):((self.N+1)*self.n + self.d*self.N + self.dimSS)] 213 | terminalCost = np.dot(self.costSS, lamb) 214 | # costSolved.append(terminalCost+(self.N+1-i)) 215 | if i == 0: 216 | costSolved.append(terminalCost+(self.N-i)) 217 | else: 218 | costSolved.append(terminalCost+(self.N-(i-1))) 219 | 220 | soluSolved.append(sol) 221 | else: 222 | costSolved.append(np.inf) 223 | soluSolved.append(sol) 224 | self.feasible = 0 225 | 226 | else: # if horizon one time step (because N-i = 0) --> just check feasibility of the initial guess 227 | uGuess = self.xGuess[(self.n*(self.N+1)):(self.n*(self.N+1)+self.d)] 228 | xNext = self.f(x0, uGuess) 229 | slackNorm.append(0.0) 230 | if self.checkTaskCompletion(xNext): 231 | self.feasible = 1 232 | costSolved.append(1) 233 | sol["x"] = self.xGuessTot 234 | soluSolved.append(sol) 235 | else: 236 | costSolved.append(np.inf) 237 | soluSolved.append(sol) 238 | self.feasible = 0 239 | 240 | 241 | # Store optimal solution 242 | self.optCost = np.min(costSolved) 243 | x = np.array(soluSolved[np.argmin(costSolved)]["x"]) 244 | self.xSol = x[0:(self.N+1)*self.n].reshape((self.N+1,self.n)).T 245 | self.uSol = x[(self.N+1)*self.n:((self.N+1)*self.n + self.d*self.N)].reshape((self.N,self.d)).T 246 | self.lamb = x[((self.N+1)*self.n+self.N*self.d):((self.N+1)*self.n + self.d*self.N + self.dimSS)] 247 | optSlack = slackNorm[np.argmin(costSolved)] 248 | 249 | def buildNonlinearProgram(self, SSVfun): 250 | # Define variables 251 | n = self.n 252 | d = self.d 253 | N = self.N 254 | X = SX.sym('X', n*(N+1)); 255 | U = SX.sym('X', d*N); 256 | dimSS = np.shape(SSVfun)[1] 257 | lamb = SX.sym('X', dimSS) 258 | xSS = SSVfun[0:n, :] 259 | costSS = np.array([SSVfun[-1, :]]) 260 | slack = SX.sym('X', n); 261 | 262 | self.dimSS = dimSS 263 | self.SSVfun = SSVfun 264 | self.xSS = xSS 265 | self.costSS = costSS 266 | self.xSS = xSS 267 | self.costSS = costSS 268 | 269 | # Define dynamic constraints 270 | constraint = [] 271 | for i in range(0, N): 272 | constraint = vertcat(constraint, X[n*(i+1)+0] - (X[n*i+0] + self.dt*X[n*i+2]*np.cos( U[d*i+0] - X[n*i+0] / self.radius) / (1 - X[n*i+1]/self.radius ) )) 273 | constraint = vertcat(constraint, X[n*(i+1)+1] - (X[n*i+1] + self.dt*X[n*i+2]*np.sin( U[d*i+0] - X[n*i+0] / self.radius) )) 274 | constraint = vertcat(constraint, X[n*(i+1)+2] - (X[n*i+2] + self.dt*U[d*i+1])) 275 | 276 | # terminal constraints 277 | constraint = vertcat(constraint, slack + X[n*N:(n*(N+1))+0] - mtimes( xSS ,lamb) ) 278 | constraint = vertcat(constraint, 1 - mtimes(np.ones((1, dimSS )), lamb ) ) 279 | 280 | # Defining Cost (We will add stage cost later) 281 | cost = mtimes(costSS, lamb) + 1000000**2*(slack[0]**2 + slack[1]**2 + slack[2]**2) 282 | self.constraint= constraint 283 | # Set IPOPT options 284 | # opts = {"verbose":False,"ipopt.print_level":0,"print_time":0,"ipopt.mu_strategy":"adaptive","ipopt.mu_init":1e-5,"ipopt.mu_min":1e-15,"ipopt.barrier_tol_factor":1}#, "ipopt.acceptable_constr_viol_tol":0.001}#,"ipopt.acceptable_tol":1e-4}#, "expand":True} 285 | opts = {"verbose":False,"ipopt.print_level":0,"print_time":0}#, "ipopt.acceptable_constr_viol_tol":0.001}#,"ipopt.acceptable_tol":1e-4}#, "expand":True} 286 | nlp = {'x':vertcat(X,U,lamb,slack), 'f':cost, 'g':constraint} 287 | self.solver = nlpsol('solver', 'ipopt', nlp, opts) 288 | 289 | # Set lower bound of inequality constraint to zero to force: 1) n*N state dynamics, 2) n terminal contraints and 3) CVX hull constraint 290 | self.lbg_dyanmics = [0]*(n*N) + [0]*(n) + [0] 291 | self.ubg_dyanmics = [0]*(n*N) + [0]*(n) + [0] 292 | 293 | 294 | 295 | def f(self, x, u): 296 | # Given a state x and input u it return the successor state 297 | xNext = np.array([x[0] + self.dt * x[2]*np.cos(u[0] - x[0]/self.radius) / (1 - x[1] / self.radius), 298 | x[1] + self.dt * x[2]*np.sin(u[0] - x[0]/self.radius), 299 | x[2] + self.dt * u[1]]) 300 | return xNext.tolist() -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex2_LMPC_for_minimum_time_unicycle/fnc/MIX_OL_CL.py: -------------------------------------------------------------------------------- 1 | class MIX_OL_CL(object): 2 | def __init__(self, radius): 3 | self.radius = radius 4 | 5 | def solve(self, x, t, totTimeSteps): 6 | self.uPred = [0, 0] 7 | if t ==0: 8 | self.uPred[1] = 0.25 9 | elif t== 1: 10 | self.uPred[1] = 0.25 11 | elif t== 2: 12 | self.uPred[1] = 0.25 13 | elif t==(totTimeSteps-5): 14 | self.uPred[1] = -0.25 15 | elif t==(totTimeSteps-4): 16 | self.uPred[1] = -0.25 17 | elif t==(totTimeSteps-3): 18 | self.uPred[1] = -0.25 19 | else: 20 | self.uPred[1] = 0 21 | 22 | self.uPred[0] = x[0] / self.radius; 23 | 24 | -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/closedloop.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/closedloop.pdf -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/fnc/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 . 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 -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/fnc/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 -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/fnc/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 -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/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([2,1]) 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[1, 0] = 1.5 * (vt - x0[0]) + np.max([-0.2, np.min([np.random.randn() * 0.10, 0.2])]) 69 | -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/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, last_laps_to_plot): 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-last_laps_to_plot, TotNumberIt): 73 | plt.plot(SS_glob[i][0:LapTime[i], 4], SS_glob[i][0:LapTime[i], 5], '-r') 74 | 75 | plt.savefig('closedloop.pdf') 76 | 77 | 78 | 79 | def plot_predicted_trajectory(map, lmpc, it, time): 80 | SS_glob = lmpc.SS_glob 81 | LapTime = lmpc.LapTime 82 | SS = lmpc.SS 83 | uSS = lmpc.uSS 84 | 85 | Points = int(np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4]))) 86 | Points1 = np.zeros((Points, 2)) 87 | Points2 = np.zeros((Points, 2)) 88 | Points0 = np.zeros((Points, 2)) 89 | 90 | for i in range(0, int(Points)): 91 | Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth) 92 | Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth) 93 | Points0[i, :] = map.getGlobalPosition(i * 0.1, 0) 94 | 95 | plt.figure(200); 96 | fig, ax = plt.subplots(); 97 | plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o') 98 | plt.plot(Points0[:, 0], Points0[:, 1], '--') 99 | plt.plot(Points1[:, 0], Points1[:, 1], '-b') 100 | plt.plot(Points2[:, 0], Points2[:, 1], '-b') 101 | plt.plot(SS_glob[it][0:LapTime[it], 4], SS_glob[it][0:LapTime[it], 5], '-ok', label="Closed-loop trajectory",zorder=-1) 102 | 103 | SSpoints_x = []; SSpoints_y = [] 104 | xPred = []; yPred = [] 105 | 106 | v = np.array([[ 1., 1.], 107 | [ 1., -1.], 108 | [-1., -1.], 109 | [-1., 1.]]) 110 | rec = patches.Polygon(v, alpha=0.7,closed=True, fc='r', ec='k',zorder=10) 111 | 112 | N = lmpc.N 113 | numSS_Points = lmpc.numSS_Points 114 | 115 | xPred = np.zeros((N+1, 1)); yPred = np.zeros((N+1, 1)) 116 | SSpoints_x = np.zeros((numSS_Points, 1)); SSpoints_y = np.zeros((numSS_Points, 1)) 117 | 118 | for j in range(0, N+1): 119 | xPred[j,0], yPred[j,0] = map.getGlobalPosition( lmpc.xStoredPredTraj[it][time][j, 4], 120 | lmpc.xStoredPredTraj[it][time][j, 5] ) 121 | 122 | if j == 0: 123 | x = SS_glob[it][time, 4] 124 | y = SS_glob[it][time, 5] 125 | psi = SS_glob[it][time, 3] 126 | l = 0.4; w = 0.2 127 | car_x = [ x + l * np.cos(psi) - w * np.sin(psi), x + l*np.cos(psi) + w * np.sin(psi), 128 | x - l * np.cos(psi) + w * np.sin(psi), x - l * np.cos(psi) - w * np.sin(psi)] 129 | car_y = [ y + l * np.sin(psi) + w * np.cos(psi), y + l * np.sin(psi) - w * np.cos(psi), 130 | y - l * np.sin(psi) - w * np.cos(psi), y - l * np.sin(psi) + w * np.cos(psi)] 131 | 132 | for j in range(0, numSS_Points): 133 | SSpoints_x[j,0], SSpoints_y[j,0] = map.getGlobalPosition(lmpc.SSStoredPredTraj[it][time][j, 4], 134 | lmpc.SSStoredPredTraj[it][time][j, 5]) 135 | plt.plot(SSpoints_x, SSpoints_y, 'sb', label="LS",zorder=0); 136 | 137 | plt.plot(xPred, yPred, '-or', label="Predicted Trajectory",zorder=1); 138 | rec.set_xy(np.array([car_x, car_y]).T); 139 | ax.add_patch(rec); 140 | 141 | plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower left", 142 | mode="expand", borderaxespad=0, ncol=3) 143 | 144 | plt.savefig('predicted.pdf') 145 | 146 | plt.show() 147 | 148 | def animation_xy(map, lmpc, it): 149 | SS_glob = lmpc.SS_glob 150 | LapTime = lmpc.LapTime 151 | SS = lmpc.SS 152 | uSS = lmpc.uSS 153 | 154 | Points = int(np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4]))) 155 | Points1 = np.zeros((Points, 2)) 156 | Points2 = np.zeros((Points, 2)) 157 | Points0 = np.zeros((Points, 2)) 158 | 159 | for i in range(0, int(Points)): 160 | Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth) 161 | Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth) 162 | Points0[i, :] = map.getGlobalPosition(i * 0.1, 0) 163 | 164 | plt.figure(200) 165 | plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o') 166 | plt.plot(Points0[:, 0], Points0[:, 1], '--') 167 | plt.plot(Points1[:, 0], Points1[:, 1], '-b') 168 | plt.plot(Points2[:, 0], Points2[:, 1], '-b') 169 | plt.plot(SS_glob[it][0:LapTime[it], 4], SS_glob[it][0:LapTime[it], 5], '-ok', label="Closed-loop trajectory",zorder=-1) 170 | 171 | ax = plt.axes() 172 | SSpoints_x = []; SSpoints_y = [] 173 | xPred = []; yPred = [] 174 | SSpoints, = ax.plot(SSpoints_x, SSpoints_y, 'sb', label="SS",zorder=0) 175 | line, = ax.plot(xPred, yPred, '-or', label="Predicted Trajectory",zorder=1) 176 | 177 | v = np.array([[ 1., 1.], 178 | [ 1., -1.], 179 | [-1., -1.], 180 | [-1., 1.]]) 181 | rec = patches.Polygon(v, alpha=0.7,closed=True, fc='r', ec='k',zorder=10) 182 | ax.add_patch(rec) 183 | 184 | plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower left", 185 | mode="expand", borderaxespad=0, ncol=3) 186 | 187 | 188 | N = lmpc.N 189 | numSS_Points = lmpc.numSS_Points 190 | for i in range(0, int(lmpc.LapTime[it])): 191 | 192 | xPred = np.zeros((N+1, 1)); yPred = np.zeros((N+1, 1)) 193 | SSpoints_x = np.zeros((numSS_Points, 1)); SSpoints_y = np.zeros((numSS_Points, 1)) 194 | 195 | for j in range(0, N+1): 196 | xPred[j,0], yPred[j,0] = map.getGlobalPosition( lmpc.xStoredPredTraj[it][i][j, 4], 197 | lmpc.xStoredPredTraj[it][i][j, 5] ) 198 | 199 | if j == 0: 200 | x = SS_glob[it][i, 4] 201 | y = SS_glob[it][i, 5] 202 | psi = SS_glob[it][i, 3] 203 | l = 0.4; w = 0.2 204 | car_x = [ x + l * np.cos(psi) - w * np.sin(psi), x + l*np.cos(psi) + w * np.sin(psi), 205 | x - l * np.cos(psi) + w * np.sin(psi), x - l * np.cos(psi) - w * np.sin(psi)] 206 | car_y = [ y + l * np.sin(psi) + w * np.cos(psi), y + l * np.sin(psi) - w * np.cos(psi), 207 | y - l * np.sin(psi) - w * np.cos(psi), y - l * np.sin(psi) + w * np.cos(psi)] 208 | 209 | for j in range(0, numSS_Points): 210 | SSpoints_x[j,0], SSpoints_y[j,0] = map.getGlobalPosition(lmpc.SSStoredPredTraj[it][i][j, 4], 211 | lmpc.SSStoredPredTraj[it][i][j, 5]) 212 | SSpoints.set_data(SSpoints_x, SSpoints_y) 213 | 214 | line.set_data(xPred, yPred) 215 | rec.set_xy(np.array([car_x, car_y]).T) 216 | plt.draw() 217 | plt.pause(1e-17) 218 | 219 | def animation_states(map, LMPCOpenLoopData, lmpc, it): 220 | SS_glob = lmpc.SS_glob 221 | LapTime = lmpc.LapTime 222 | SS = lmpc.SS 223 | uSS = lmpc.uSS 224 | 225 | xdata = []; ydata = [] 226 | fig = plt.figure(100) 227 | 228 | axvx = fig.add_subplot(3, 2, 1) 229 | plt.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 0, it], '-ok', label="Closed-loop trajectory") 230 | lineSSvx, = axvx.plot(xdata, ydata, 'sb-', label="SS") 231 | linevx, = axvx.plot(xdata, ydata, 'or-', label="Predicted Trajectory") 232 | plt.ylabel("vx") 233 | plt.xlabel("s") 234 | 235 | plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower left", 236 | mode="expand", borderaxespad=0, ncol=3) 237 | 238 | axvy = fig.add_subplot(3, 2, 2) 239 | axvy.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 1, it], '-ok') 240 | lineSSvy, = axvy.plot(xdata, ydata, 'sb-') 241 | linevy, = axvy.plot(xdata, ydata, 'or-') 242 | plt.ylabel("vy") 243 | plt.xlabel("s") 244 | 245 | axwz = fig.add_subplot(3, 2, 3) 246 | axwz.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 2, it], '-ok') 247 | lineSSwz, = axwz.plot(xdata, ydata, 'sb-') 248 | linewz, = axwz.plot(xdata, ydata, 'or-') 249 | plt.ylabel("wz") 250 | plt.xlabel("s") 251 | 252 | axepsi = fig.add_subplot(3, 2, 4) 253 | axepsi.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 3, it], '-ok') 254 | lineSSepsi, = axepsi.plot(xdata, ydata, 'sb-') 255 | lineepsi, = axepsi.plot(xdata, ydata, 'or-') 256 | plt.ylabel("epsi") 257 | plt.xlabel("s") 258 | 259 | axey = fig.add_subplot(3, 2, 5) 260 | axey.plot(SS[0:LapTime[it], 4, it], SS[0:LapTime[it], 5, it], '-ok') 261 | lineSSey, = axey.plot(xdata, ydata, 'sb-') 262 | lineey, = axey.plot(xdata, ydata, 'or-') 263 | plt.ylabel("ey") 264 | plt.xlabel("s") 265 | 266 | Points = np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4])) 267 | Points1 = np.zeros((Points, 2)) 268 | Points2 = np.zeros((Points, 2)) 269 | Points0 = np.zeros((Points, 2)) 270 | for i in range(0, int(Points)): 271 | Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth) 272 | Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth) 273 | Points0[i, :] = map.getGlobalPosition(i * 0.1, 0) 274 | 275 | axtr = fig.add_subplot(3, 2, 6) 276 | plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o') 277 | plt.plot(Points0[:, 0], Points0[:, 1], '--') 278 | plt.plot(Points1[:, 0], Points1[:, 1], '-b') 279 | plt.plot(Points2[:, 0], Points2[:, 1], '-b') 280 | plt.plot(SS_glob[it][0:LapTime[it], 4], SS_glob[it][0:LapTime[it], 5], '-ok') 281 | 282 | SSpoints_x = []; SSpoints_y = [] 283 | xPred = []; yPred = [] 284 | SSpoints_tr, = axtr.plot(SSpoints_x, SSpoints_y, 'sb') 285 | line_tr, = axtr.plot(xPred, yPred, '-or') 286 | 287 | N = lmpc.N 288 | numSS_Points = lmpc.numSS_Points 289 | for i in range(0, int(lmpc.LapTime[it])): 290 | 291 | xPred = lmpc.xStoredPredTraj[it][i] 292 | SSpoints = lmpc.SSStoredPredTraj[it][i] 293 | 294 | linevx.set_data(xPred[:, 4], xPred[:, 0]); axvx.set_title(str(xPred[0, 0])) 295 | linevy.set_data(xPred[:, 4], xPred[:, 1]); axvy.set_title(str(xPred[0, 1])) 296 | linewz.set_data(xPred[:, 4], xPred[:, 2]); axwz.set_title(str(xPred[0, 2])) 297 | lineepsi.set_data(xPred[:, 4], xPred[:, 3]); axepsi.set_title(str(xPred[0, 3])) 298 | lineey.set_data(xPred[:, 4], xPred[:, 5]); axey.set_title(str(xPred[0, 5])) 299 | 300 | epsiReal = xPred[0, 3] 301 | 302 | lineSSvx.set_data(SSpoints[4,:], SSpoints[0,:]) 303 | lineSSvy.set_data(SSpoints[4,:], SSpoints[1,:]) 304 | lineSSwz.set_data(SSpoints[4,:], SSpoints[2,:]) 305 | lineSSepsi.set_data(SSpoints[4,:], SSpoints[3,:]) 306 | lineSSey.set_data(SSpoints[4,:], SSpoints[5,:]) 307 | 308 | xPred = np.zeros((N + 1, 1));yPred = np.zeros((N + 1, 1)) 309 | SSpoints_x = np.zeros((numSS_Points, 1));SSpoints_y = np.zeros((numSS_Points, 1)) 310 | 311 | for j in range(0, N + 1): 312 | xPred[j, 0], yPred[j, 0] = map.getGlobalPosition(lmpc.xStoredPredTraj[it][i][j, 4], 313 | lmpc.xStoredPredTraj[it][i][j, 5]) 314 | 315 | for j in range(0, numSS_Points): 316 | SSpoints_x[j, 0], SSpoints_y[j, 0] = map.getGlobalPosition(LMPCOpenLoopData.SSused[4, j, i, it], 317 | LMPCOpenLoopData.SSused[5, j, i, it]) 318 | 319 | line_tr.set_data(xPred, yPred) 320 | 321 | 322 | vec = np.array([xPred[0, 0], yPred[0, 0]]) - np.array([SS_glob[i, 4, it], SS_glob[i, 5, it]]) 323 | 324 | s, ey, epsi, _ = map.getLocalPosition( SS_glob[i, 4, it], SS_glob[i, 5, it], SS_glob[i, 3, it]) 325 | axtr.set_title(str(s)+" "+str(ey)+" "+str(epsi)) 326 | 327 | # axepsi.set_title(str(epsiReal)+" "+str(epsi)) 328 | SSpoints_tr.set_data(SSpoints_x, SSpoints_y) 329 | 330 | plt.draw() 331 | plt.pause(1e-17) 332 | 333 | def saveGif_xyResults(map, LMPCOpenLoopData, lmpc, it): 334 | SS_glob = lmpc.SS_glob 335 | LapTime = lmpc.LapTime 336 | SS = lmpc.SS 337 | uSS = lmpc.uSS 338 | 339 | Points = int(np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4]))) 340 | Points1 = np.zeros((Points, 2)) 341 | Points2 = np.zeros((Points, 2)) 342 | Points0 = np.zeros((Points, 2)) 343 | for i in range(0, int(Points)): 344 | Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth) 345 | Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth) 346 | Points0[i, :] = map.getGlobalPosition(i * 0.1, 0) 347 | 348 | fig = plt.figure(101) 349 | # plt.ylim((-5, 1.5)) 350 | fig.set_tight_layout(True) 351 | plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o') 352 | plt.plot(Points0[:, 0], Points0[:, 1], '--') 353 | plt.plot(Points1[:, 0], Points1[:, 1], '-b') 354 | plt.plot(Points2[:, 0], Points2[:, 1], '-b') 355 | 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) 356 | 357 | ax = plt.axes() 358 | SSpoints_x = []; SSpoints_y = [] 359 | xPred = []; yPred = [] 360 | SSpoints, = ax.plot(SSpoints_x, SSpoints_y, 'og', label="SS",zorder=0) 361 | line, = ax.plot(xPred, yPred, '-or', label="Predicted Trajectory",zorder=1) 362 | 363 | v = np.array([[ 1., 1.], 364 | [ 1., -1.], 365 | [-1., -1.], 366 | [-1., 1.]]) 367 | rec = patches.Polygon(v, alpha=0.7,closed=True, fc='g', ec='k',zorder=10) 368 | ax.add_patch(rec) 369 | 370 | plt.legend(mode="expand", ncol=3) 371 | plt.title('Lap: '+str(it)) 372 | # plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower left", 373 | # mode="expand", borderaxespad=0, ncol=3) 374 | 375 | N = lmpc.N 376 | numSS_Points = lmpc.numSS_Points 377 | 378 | def update(i): 379 | xPred = np.zeros((N + 1, 1)); yPred = np.zeros((N + 1, 1)) 380 | SSpoints_x = np.zeros((numSS_Points, 1)); SSpoints_y = np.zeros((numSS_Points, 1)) 381 | 382 | for j in range(0, N + 1): 383 | xPred[j, 0], yPred[j, 0] = map.getGlobalPosition(lmpc.xStoredPredTraj[it][i][j, 4], 384 | lmpc.xStoredPredTraj[it][i][j, 5]) 385 | 386 | if j == 0: 387 | x = SS_glob[i, 4, it] 388 | y = SS_glob[i, 5, it] 389 | psi = SS_glob[i, 3, it] 390 | l = 0.4;w = 0.2 391 | car_x = [x + l * np.cos(psi) - w * np.sin(psi), x + l * np.cos(psi) + w * np.sin(psi), 392 | x - l * np.cos(psi) + w * np.sin(psi), x - l * np.cos(psi) - w * np.sin(psi)] 393 | car_y = [y + l * np.sin(psi) + w * np.cos(psi), y + l * np.sin(psi) - w * np.cos(psi), 394 | y - l * np.sin(psi) - w * np.cos(psi), y - l * np.sin(psi) + w * np.cos(psi)] 395 | 396 | for j in range(0, numSS_Points): 397 | SSpoints_x[j, 0], SSpoints_y[j, 0] = map.getGlobalPosition(LMPCOpenLoopData.SSused[4, j, i, it], 398 | LMPCOpenLoopData.SSused[5, j, i, it]) 399 | SSpoints.set_data(SSpoints_x, SSpoints_y) 400 | 401 | line.set_data(xPred, yPred) 402 | 403 | rec.set_xy(np.array([car_x, car_y]).T) 404 | 405 | anim = FuncAnimation(fig, update, frames=np.arange(0, int(lmpc.LapTime[it])), interval=100) 406 | 407 | anim.save('ClosedLoop.gif', dpi=80, writer='imagemagick') 408 | -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/initControllerParameters.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from fnc.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 -------------------------------------------------------------------------------- /Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/predicted.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/Nominal_LMPC_Chapter/Ex3_LMPC_for_autonomous_racing/predicted.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Learning Robust MPC 2 | 3 | This code repo is .... 4 | 5 | # Prerequisite 6 | 7 | Please create the py3 conda environment running the following commands 8 | ``` 9 | conda env create --file py3.yml 10 | ``` 11 | -------------------------------------------------------------------------------- /Stochastic_Safe_Sets_Chapter/Ex1_Sample_based_safe_set/fnc/build_control_invariant.py: -------------------------------------------------------------------------------- 1 | from casadi import * 2 | from numpy import * 3 | import pdb 4 | import itertools 5 | import numpy as np 6 | import time 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | from scipy.spatial import ConvexHull 10 | from cvxopt import matrix, solvers 11 | import pypoman 12 | import scipy 13 | solvers.options['show_progress'] = False 14 | from scipy.optimize import linprog 15 | 16 | class BuildControlInvariant(object): 17 | 18 | def __init__(self, A, B, maxIt, maxTime, maxRollOut, sys, mpc, x_data_O_estimated, u_data_O_estimated, store_all_data_flag=False): 19 | # Data roll-outs current iteration 20 | self.x_data = x_data_O_estimated # list of stored closedLoop 21 | self.u_data = u_data_O_estimated # list of stored closedLoop 22 | 23 | self.x_cl_data = [] 24 | self.u_cl_data = [] 25 | 26 | self.store_all_data_flag = store_all_data_flag 27 | 28 | self.A = A 29 | self.B = B 30 | 31 | self.mpc = mpc 32 | 33 | self.maxIt = maxIt 34 | self.maxTime = maxTime 35 | self.sys = sys 36 | self.maxRollOut= maxRollOut 37 | 38 | 39 | def build_control_invariant(self): 40 | # Compute robust invariant from data 41 | x_cl = []; u_cl = []; 42 | 43 | for it in range(0,self.maxIt): 44 | for rollOut in range(0, self.maxRollOut): # Roll-out loop 45 | self.sys.reset_IC() # Reset initial conditions 46 | print("Start roll out: ", rollOut, " of iteration: ", it) 47 | for t in range(0,self.maxTime): # Time loop 48 | ut = self.mpc.solve(self.sys.x[-1]) 49 | self.sys.applyInput(ut) 50 | 51 | # Closed-loop trajectory. The convention is row = time, col = state 52 | x_cl.append(np.array(self.sys.x)) 53 | u_cl.append(np.array(self.sys.u)) 54 | self.add_data(self.sys.x, self.sys.u) 55 | 56 | if self.store_all_data_flag == True: 57 | self.x_cl_data.append(np.array(self.sys.x)) 58 | self.u_cl_data.append(np.array(self.sys.u)) 59 | 60 | 61 | if self.check_control_invariance(): 62 | print("Control invariant found") 63 | break 64 | else: 65 | print("Control invariant NOT found") 66 | self.x_cl = x_cl 67 | self.u_cl = u_cl 68 | 69 | def add_data(self, x_cl, u_cl): 70 | for x, u in zip(x_cl, u_cl): 71 | if len(self.x_data)>0: 72 | contained = self.check_if_in_cvx_hull(x) 73 | if contained == False: 74 | self.x_data.append(x) 75 | self.u_data.append(u) 76 | else: 77 | self.x_data.append(x) 78 | self.u_data.append(u) 79 | 80 | def check_if_in_cvx_hull(self, x): 81 | points = np.array(self.x_data) 82 | n_points = len(points) 83 | c = np.zeros(n_points) 84 | A = np.r_[points.T,np.ones((1,n_points))] 85 | b = np.r_[x, np.ones(1)] 86 | lp = linprog(c, A_eq=A, b_eq=b) 87 | return lp.success 88 | 89 | def check_control_invariance(self): 90 | control_invariant = True 91 | for x, u in zip(self.x_data, self.u_data): 92 | x_next = np.dot(self.A, x) + np.dot(self.B, u) 93 | contained = self.check_if_in_cvx_hull(x_next) 94 | if not contained: 95 | control_invariant = False 96 | return control_invariant 97 | 98 | return control_invariant -------------------------------------------------------------------------------- /Stochastic_Safe_Sets_Chapter/Ex1_Sample_based_safe_set/fnc/build_robust_invariant.py: -------------------------------------------------------------------------------- 1 | from casadi import * 2 | from numpy import * 3 | import pdb 4 | import itertools 5 | import numpy as np 6 | import time 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | from scipy.spatial import ConvexHull 10 | from cvxopt import matrix, solvers 11 | import pypoman 12 | import scipy 13 | solvers.options['show_progress'] = False 14 | from scipy.optimize import linprog 15 | 16 | class BuildRobustInvariant(object): 17 | 18 | def __init__(self, barA, barB, A, B, Q, R, bx, bu, verticesW, maxIt, maxTime, maxRollOut, sys_aug): 19 | # Data roll-outs current iteration 20 | self.x_data = [] # list of stored closedLoop 21 | self.u_data = [] # list of stored closedLoop 22 | 23 | self.A = A 24 | self.B = B 25 | self.barA = barA 26 | self.barB = barB 27 | self.bx = bx 28 | self.bu = bu 29 | self.Q = Q 30 | self.R = R 31 | self.maxIt = maxIt 32 | self.maxTime = maxTime 33 | self.sys_aug = sys_aug 34 | self.maxRollOut= maxRollOut 35 | 36 | self.itCounter = 0 37 | 38 | self.verticesW = verticesW 39 | self.computeRobutInvariant() 40 | self.A_O, self.b_O = pypoman.duality.compute_polytope_halfspaces(self.verticesO) 41 | self.A_W, self.b_W = pypoman.duality.compute_polytope_halfspaces(self.verticesW) 42 | 43 | def build_robust_invariant(self): 44 | # Compute robust invariant from data 45 | x_cl = []; u_cl = []; 46 | 47 | for it in range(0,self.maxIt): 48 | for rollOut in range(0, self.maxRollOut): # Roll-out loop 49 | self.sys_aug.reset_IC() # Reset initial conditions 50 | print("Start roll out: ", rollOut, " of iteration: ", it) 51 | for t in range(0,self.maxTime): # Time loop 52 | # ut = mpc.solve(sys.x[-1]) 53 | ut = -np.dot(self.K, self.sys_aug.x[-1]) 54 | self.sys_aug.applyInput(ut) 55 | 56 | # Closed-loop trajectory. The convention is row = time, col = state 57 | x_cl.append(np.array(self.sys_aug.x)) 58 | self.sys_aug.u.append(-self.K@self.sys_aug.x[-1]) 59 | u_cl.append(np.array(self.sys_aug.u)) 60 | self.add_data(self.sys_aug.x, self.sys_aug.u) 61 | # TO DO: check if trajectory reached the goal 62 | # impc.addData(x_cl[-1], u_cl[-1]) # Add data while performing the task 63 | if self.check_robust_invariance(): 64 | print("Robust invariant found") 65 | break 66 | else: 67 | print("Robust invariant NOT found") 68 | self.x_cl = x_cl 69 | self.u_cl = u_cl 70 | 71 | def add_data(self, x_cl, u_cl): 72 | for x, u in zip(x_cl, u_cl): 73 | if len(self.x_data)>0: 74 | contained = self.check_if_in_cvx_hull(x) 75 | if contained == False: 76 | self.x_data.append(x) 77 | self.u_data.append(u) 78 | else: 79 | self.x_data.append(x) 80 | self.u_data.append(u) 81 | 82 | def check_if_in_cvx_hull(self, x): 83 | points = np.array(self.x_data) 84 | n_points = len(points) 85 | c = np.zeros(n_points) 86 | A = np.r_[points.T,np.ones((1,n_points))] 87 | b = np.r_[x, np.ones(1)] 88 | lp = linprog(c, A_eq=A, b_eq=b) 89 | return lp.success 90 | 91 | def check_robust_invariance(self): 92 | robust_invariant = True 93 | for x in self.x_data: 94 | for w in self.verticesW: 95 | x_next = np.dot(self.barAcl,x) + w 96 | contained = self.check_if_in_cvx_hull(x_next) 97 | if not contained: 98 | robust_invariant = False 99 | return robust_invariant 100 | 101 | return robust_invariant 102 | 103 | def computeRobutInvariant(self): 104 | self.O_v = [np.array([0,0])] 105 | self.dlqr() 106 | print("Compute robust invariant") 107 | # TO DO: 108 | # - add check for convergence 109 | # - add check for input and state constraint satifaction 110 | for i in range(0,20): 111 | self.O_v = self.MinkowskiSum(self.O_v, self.verticesW.tolist()) 112 | 113 | self.verticesO = np.array(self.O_v) 114 | 115 | def MinkowskiSum(self, setA, setB): 116 | vertices = [] 117 | for v1 in setA: 118 | for v2 in setB: 119 | vertices.append(np.dot(self.Acl,v1) + v2) 120 | 121 | cvxHull = ConvexHull(vertices) 122 | verticesOut = [] 123 | for idx in cvxHull.vertices: 124 | verticesOut.append(vertices[idx]) 125 | 126 | return verticesOut 127 | 128 | def dlqr(self): 129 | # solve the ricatti equation 130 | P = np.matrix(scipy.linalg.solve_discrete_are(self.A, self.B, self.Q, self.R)) 131 | # compute the LQR gain 132 | self.K = np.array(scipy.linalg.inv(self.B.T*P*self.B+self.R)*(self.B.T*P*self.A)) 133 | self.Acl = self.A - np.dot(self.B, self.K) 134 | self.barAcl = self.barA - np.dot(self.barB, self.K) 135 | 136 | def shrink_constraint(self): 137 | array_data = np.array(self.x_data).T 138 | self.bx_shrink = [] 139 | self.bx_shrink.append(self.bx[0]-array_data.max(axis=1)) 140 | self.bx_shrink.append(self.bx[1]-array_data.min(axis=1)) 141 | 142 | print("shrunk state constraints max: ", self.bx_shrink[0]) 143 | print("shrunk state constraints min: ", self.bx_shrink[1]) 144 | 145 | array_data_kx = np.dot(self.K, array_data) 146 | 147 | self.bu_shrink = [] 148 | self.bu_shrink.append(self.bu[0]-array_data_kx.max(axis=1)) 149 | self.bu_shrink.append(self.bu[1]-array_data_kx.min(axis=1)) 150 | 151 | print("shrunk input constraints max: ", self.bu_shrink[0]) 152 | print("shrunk input constraints min: ", self.bu_shrink[1]) 153 | -------------------------------------------------------------------------------- /Stochastic_Safe_Sets_Chapter/Ex1_Sample_based_safe_set/fnc/mpc.py: -------------------------------------------------------------------------------- 1 | from casadi import * 2 | from numpy import * 3 | import pdb 4 | import itertools 5 | import numpy as np 6 | from cvxpy import * 7 | import time 8 | 9 | class MPC(object): 10 | """ Finite Time Optimal Control Problem (FTOCP) 11 | Methods: 12 | - solve: solves the FTOCP given the initial condition x0 and terminal contraints 13 | - buildNonlinearProgram: builds the ftocp program solved by the above solve method 14 | - model: given x_t and u_t computes x_{t+1} = f( x_t, u_t ) 15 | 16 | """ 17 | 18 | def __init__(self, N, A, B, Q, R, Qf, bx, bu, K, verticesO, verticesXf): 19 | # Define variables 20 | self.A = A 21 | self.B = B 22 | self.N = N 23 | self.n = A.shape[1] 24 | self.d = B.shape[1] 25 | self.bx = bx 26 | self.bu = bu 27 | self.Q = Q 28 | self.Qf = Qf 29 | self.R = R 30 | self.K = K 31 | self.verticesO = verticesO 32 | self.verticesXf = verticesXf 33 | 34 | print("Initializing FTOCP") 35 | self.buildFTOCP() 36 | self.solverTime = [] 37 | print("Done initializing FTOCP") 38 | 39 | def solve(self, x0, verbose=False): 40 | # Set initial condition + state and input box constraints 41 | self.lbx = x0.tolist() + (self.bx[1]).tolist()*(self.N+1) + (self.bu[1]).tolist()*self.N + [0]*self.verticesO.shape[0] + [0]*self.verticesXf.shape[0] 42 | self.ubx = x0.tolist() + (self.bx[0]).tolist()*(self.N+1) + (self.bu[0]).tolist()*self.N + [1]*self.verticesO.shape[0] + [1]*self.verticesXf.shape[0] 43 | 44 | # Solve nonlinear programm 45 | start = time.time() 46 | sol = self.solver(lbx=self.lbx, ubx=self.ubx, lbg=self.lbg_dyanmics, ubg=self.ubg_dyanmics) 47 | end = time.time() 48 | self.solverTime = end - start 49 | # print("solver time: ", self.solverTime) 50 | 51 | # Check if the solution is feasible 52 | if (self.solver.stats()['success']): 53 | self.feasible = 1 54 | x = sol["x"] 55 | self.xt = np.array(x[0:self.n]) 56 | self.xPred = np.array(x[self.n:(self.N+2)*self.n].reshape((self.n,self.N+1))).T 57 | self.uPred = np.array(x[(self.N+2)*self.n:((self.N+2)*self.n + self.d*self.N)].reshape((self.d,self.N))).T 58 | self.lambda_xt = np.array(x[((self.N+2)*self.n + self.d*self.N):((self.N+2)*self.n + self.d*self.N)+self.verticesO.shape[0]]) 59 | self.lambda_xf = np.array(x[((self.N+2)*self.n + self.d*self.N)+self.verticesO.shape[0]:((self.N+2)*self.n + self.d*self.N)+self.verticesO.shape[0]+self.verticesXf.shape[0]]) 60 | 61 | self.mpcInput = self.uPred[0][0] - self.K @ (self.xt.squeeze() - self.xPred[0,:]) 62 | else: 63 | self.xPred = np.zeros((self.N+1,self.n) ) 64 | self.uPred = np.zeros((self.N,self.d)) 65 | self.mpcInput = [] 66 | self.feasible = 0 67 | print("Unfeasible") 68 | 69 | return self.mpcInput 70 | 71 | def buildFTOCP(self): 72 | # Define variables 73 | n = self.n 74 | d = self.d 75 | 76 | # Define variables 77 | Xt = SX.sym('Xt', n) 78 | X = SX.sym('X', n*(self.N+1)) 79 | U = SX.sym('U', d*self.N) 80 | lamb_xt = SX.sym('lamb_xt', self.verticesO.shape[0]) 81 | lamb_xf = SX.sym('lamb_xt', self.verticesXf.shape[0]) 82 | 83 | # Define dynamic constraints 84 | self.constraint = [] 85 | for i in range(0, self.N): 86 | X_next = self.dynamics(X[n*i:n*(i+1)], U[d*i:d*(i+1)]) 87 | for j in range(0, self.n): 88 | self.constraint = vertcat(self.constraint, X_next[j] - X[n*(i+1)+j] ) 89 | 90 | # Initial constraint x{t|t} - x(t) \in \mathcal{O} 91 | self.constraint = vertcat(self.constraint, Xt - X[0:n] - mtimes( self.verticesO.T ,lamb_xt) ) 92 | self.constraint = vertcat(self.constraint, 1 - mtimes(np.ones((1, self.verticesO.shape[0] )), lamb_xt ) ) 93 | 94 | # Enforce terminal constraint 95 | self.constraint = vertcat(self.constraint, X[n*self.N:(n*(self.N+1))] - mtimes( self.verticesXf.T ,lamb_xf) ) 96 | self.constraint = vertcat(self.constraint, 1 - mtimes(np.ones((1, self.verticesXf.shape[0])), lamb_xf ) ) 97 | 98 | # Defining Cost (We will add terminal cost later) 99 | # self.cost = (self.K @ (Xt - X[0:n])).T @ self.R @ (self.K @ (Xt - X[0:n])) 100 | # self.cost = self.cost + (Xt - X[0:n]).T @ self.Q @ (Xt - X[0:n]) 101 | self.cost = 0 102 | for i in range(0, self.N): 103 | self.cost = self.cost + X[n*i:n*(i+1)].T @ self.Q @ X[n*i:n*(i+1)] 104 | self.cost = self.cost + U[d*i:d*(i+1)].T @ self.R @ U[d*i:d*(i+1)] 105 | 106 | self.cost = self.cost + X[n*self.N:n*(self.N+1)].T @ self.Qf @ X[n*self.N:n*(self.N+1)] 107 | 108 | # Set IPOPT options 109 | # opts = {"verbose":False,"ipopt.print_level":0,"print_time":0,"ipopt.mu_strategy":"adaptive","ipopt.mu_init":1e-5,"ipopt.mu_min":1e-15,"ipopt.barrier_tol_factor":1}#, "ipopt.acceptable_constr_viol_tol":0.001}#,"ipopt.acceptable_tol":1e-4}#, "expand":True} 110 | opts = {"verbose":False,"ipopt.print_level":0,"print_time":0}#\\, "ipopt.acceptable_constr_viol_tol":0.001}#,"ipopt.acceptable_tol":1e-4}#, "expand":True} 111 | nlp = {'x':vertcat(Xt, X, U, lamb_xt, lamb_xf), 'f':self.cost, 'g':self.constraint} 112 | self.solver = nlpsol('solver', 'ipopt', nlp, opts) 113 | 114 | # Set lower bound of inequality constraint to zero to force n*N state dynamics, n+1 for initial constraint, and [0]*(n+1) for terminal constraint 115 | self.lbg_dyanmics = [0]*(n*self.N) + [0]*(n+1) + [0]*(n+1) 116 | self.ubg_dyanmics = [0]*(n*self.N) + [0]*(n+1) + [0]*(n+1) 117 | 118 | def dynamics(self, x, u): 119 | return self.A @ x + self.B @ u 120 | -------------------------------------------------------------------------------- /Stochastic_Safe_Sets_Chapter/Ex1_Sample_based_safe_set/invariant.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/Stochastic_Safe_Sets_Chapter/Ex1_Sample_based_safe_set/invariant.pdf -------------------------------------------------------------------------------- /py3.yml: -------------------------------------------------------------------------------- 1 | name: py3 2 | channels: 3 | - conda-forge 4 | - bioconda 5 | - defaults 6 | dependencies: 7 | - ampl-mp=3.1.0=h131c384_1004 8 | - ca-certificates=2020.12.5=h033912b_0 9 | - casadi=3.5.5=py38h98bb4e1_2 10 | - certifi=2020.12.5=py38h50d1736_0 11 | - cvxopt=1.2.5=py38h1de7c78_1 12 | - cvxpy=1.1.7=py38h11c0d25_0 13 | - cvxpy-base=1.1.7=py38h6be0db6_0 14 | - dill=0.3.3=pyhd8ed1ab_0 15 | - dsdp=5.8=h7674d01_1203 16 | - ecos=2.0.7.post1=py38hf3d72b9_3 17 | - fftw=3.3.8=nompi_h44ae4c8_1111 18 | - future=0.18.2=py38h32f6830_1 19 | - glpk=4.65=h65ac59c_1002 20 | - gmp=6.2.0=hb1e8313_2 21 | - gsl=2.6=ha2d443c_0 22 | - ipopt=3.13.3=h4cf99db_3 23 | - libblas=3.8.0=17_openblas 24 | - libcblas=3.8.0=17_openblas 25 | - libcxx=10.0.1=h5f48129_0 26 | - libffi=3.2.1=hb1e8313_1007 27 | - libgfortran=4.0.0=h50e675f_13 28 | - libgfortran4=7.5.0=h50e675f_13 29 | - liblapack=3.8.0=17_openblas 30 | - libopenblas=0.3.10=openmp_h63d9170_4 31 | - llvm-openmp=10.0.1=h28b9765_0 32 | - metis=5.1.0=hb1e8313_1006 33 | - mumps-include=5.2.1=h694c41f_10 34 | - mumps-seq=5.2.1=h0fefb41_10 35 | - ncurses=6.2=hb1e8313_1 36 | - numpy=1.19.1=py38h8ccc501_2 37 | - openssl=1.1.1h=haf1e3a3_0 38 | - osqp=0.6.1=py38h5fc983b_2 39 | - pip=20.2.2=py_0 40 | - python=3.8.5=hfc71d35_5_cpython 41 | - python_abi=3.8=1_cp38 42 | - readline=8.0=h0678c8f_2 43 | - scotch=6.0.9=h57311db_1 44 | - scs=2.1.2=py38hd88393d_2 45 | - setuptools=49.6.0=py38h32f6830_0 46 | - sqlite=3.33.0=h960bd1c_0 47 | - suitesparse=5.7.2=h0e59142_0 48 | - tbb=2019.9=ha1b3eb9_1 49 | - tk=8.6.10=hb0a8c7a_0 50 | - wheel=0.35.1=pyh9f0ad1d_0 51 | - xz=5.2.5=haf1e3a3_1 52 | - zlib=1.2.11=1007 53 | - pip: 54 | - appnope==0.1.3 55 | - argon2-cffi==21.3.0 56 | - argon2-cffi-bindings==21.2.0 57 | - asttokens==2.0.5 58 | - attrs==21.4.0 59 | - backcall==0.2.0 60 | - beautifulsoup4==4.11.1 61 | - bleach==5.0.0 62 | - cffi==1.15.0 63 | - cycler==0.10.0 64 | - debugpy==1.6.0 65 | - decorator==5.1.1 66 | - defusedxml==0.7.1 67 | - entrypoints==0.4 68 | - executing==0.8.3 69 | - fastjsonschema==2.15.3 70 | - gurobipy==9.1.2 71 | - importlib-resources==5.7.1 72 | - ipykernel==6.13.0 73 | - ipython==8.4.0 74 | - ipython-genutils==0.2.0 75 | - ipywidgets==7.7.0 76 | - jedi==0.18.1 77 | - jinja2==3.1.2 78 | - jsonschema==4.5.1 79 | - jupyter==1.0.0 80 | - jupyter-client==7.3.1 81 | - jupyter-console==6.4.3 82 | - jupyter-core==4.10.0 83 | - jupyterlab-pygments==0.2.2 84 | - jupyterlab-widgets==1.1.0 85 | - kiwisolver==1.2.0 86 | - lab==7.0 87 | - markupsafe==2.1.1 88 | - matplotlib==3.3.1 89 | - matplotlib-inline==0.1.3 90 | - mistune==0.8.4 91 | - nbclient==0.6.3 92 | - nbconvert==6.5.0 93 | - nbformat==5.4.0 94 | - nest-asyncio==1.5.5 95 | - notebook==6.4.11 96 | - packaging==21.3 97 | - pandocfilters==1.5.0 98 | - parso==0.8.3 99 | - pexpect==4.8.0 100 | - pickleshare==0.7.5 101 | - pillow==7.2.0 102 | - prometheus-client==0.14.1 103 | - prompt-toolkit==3.0.29 104 | - psutil==5.9.1 105 | - ptyprocess==0.7.0 106 | - pure-eval==0.2.2 107 | - pycddlib==2.1.2 108 | - pycparser==2.21 109 | - pygments==2.12.0 110 | - pyparsing==2.4.7 111 | - pypoman==0.5.4 112 | - pyrsistent==0.18.1 113 | - python-dateutil==2.8.1 114 | - pyzmq==23.0.0 115 | - qtconsole==5.3.0 116 | - qtpy==2.1.0 117 | - scipy==1.5.0 118 | - send2trash==1.8.0 119 | - simplejson==3.17.6 120 | - six==1.15.0 121 | - soupsieve==2.3.2.post1 122 | - stack-data==0.2.0 123 | - terminado==0.15.0 124 | - tinycss2==1.1.1 125 | - tornado==6.1 126 | - traitlets==5.2.1.post0 127 | - txt2tags==3.7 128 | - wcwidth==0.2.5 129 | - webencodings==0.5.1 130 | - widgetsnbextension==3.6.0 131 | - zipp==3.8.0 132 | prefix: /opt/anaconda3/envs/py3 133 | -------------------------------------------------------------------------------- /utils/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urosolia/Learning_Robust_MPC/cd6e1329e8447db4618c4fe00f2b1adfbccd154e/utils/.DS_Store -------------------------------------------------------------------------------- /utils/.gitignore: -------------------------------------------------------------------------------- 1 | *pcy -------------------------------------------------------------------------------- /utils/polytope.py: -------------------------------------------------------------------------------- 1 | import pypoman 2 | from numpy import array, eye, ones, vstack, zeros 3 | import numpy as np 4 | import scipy 5 | import pdb 6 | import matplotlib.pyplot as plt 7 | from matplotlib.patches import Polygon 8 | from scipy.spatial import ConvexHull, convex_hull_plot_2d 9 | 10 | class polytope(object): 11 | """docstring for polytope""" 12 | def __init__(self, F=None, b=None, vertices=None): 13 | self.maxIt = 100 14 | if vertices is None: 15 | self.b = b 16 | self.F = F 17 | self.computeVRep() 18 | else: 19 | hull= ConvexHull(vertices) 20 | self.vertices = vertices[hull.vertices, :] 21 | self.vertices = np.round(self.vertices, 6) 22 | 23 | try: 24 | self.computeHRep() 25 | self.computeVRep() 26 | except: 27 | print("Numerical inaccuracy while computing HRep and/or VRep. Rounding vertices to 6th significant digit.") 28 | self.vertices = np.round(self.vertices, 6) 29 | self.computeHRep() 30 | self.computeVRep() 31 | 32 | 33 | def NStepPreAB(self, A, B, Fx, bx, Fu, bu, N): 34 | 35 | for i in range(0, N): 36 | FPreAB, bPreAB = self.preAB(A, B, Fu, bu) 37 | FPreAB = np.vstack((FPreAB, Fx)) 38 | bPreAB = np.hstack((bPreAB, bx)) 39 | self.F = FPreAB 40 | self.b = bPreAB 41 | 42 | def computeO_inf(self, A): 43 | terminated = False 44 | for i in range(0,self.maxIt): 45 | F1 = self.F 46 | b1 = self.b 47 | Fpre, bpre = self.preA(A) 48 | self.intersect(Fpre, bpre) 49 | # if self.contained(self.F, self.b, self.F, self.b): 50 | if self.contained(F1, b1, self.F, self.b) and self.contained(self.F, self.b, F1, b1): 51 | terminated = True 52 | print("Oinfinity computation terminated after ",i," iterations") 53 | break 54 | if terminated == False: 55 | print("Oinfinity computation has not terminated after ",self.maxIt," iterations") 56 | 57 | def contained(self, F,b, F1, b1): 58 | vertices = pypoman.duality.compute_polytope_vertices(F, b) 59 | vertices1 = pypoman.duality.compute_polytope_vertices(F1, b1) 60 | contained = True 61 | for v in vertices: 62 | if (np.dot(F1, v.T) > b1 + 10**(-12)).any(): 63 | contained = False 64 | return contained 65 | 66 | def computeC_inf(self, A, B, Fu, bu): 67 | terminated = False 68 | for i in range(0,self.maxIt): 69 | F1 = self.F 70 | b1 = self.b 71 | Fpre, bpre = self.preAB(A, B, Fu, bu) 72 | self.intersect(Fpre, bpre) 73 | if self.contained(F1, b1, self.F, self.b) and self.contained(self.F, self.b, F1, b1): 74 | terminated = True 75 | print("Cinfinity computation terminated after ",i," iterations") 76 | break 77 | 78 | if terminated == False: 79 | print("Cinfinity computation has not terminated after ",self.maxIt," iterations") 80 | 81 | def preA(self, A): 82 | b = self.b 83 | F = np.dot(self.F, A) 84 | return F, b 85 | 86 | def intersect(self, F_intersect, b_intersect): 87 | self.F = np.vstack((self.F, F_intersect)) 88 | self.b = np.hstack((self.b, b_intersect)) 89 | 90 | def computeVRep(self, verbose = False): 91 | self.vertices = pypoman.duality.compute_polytope_vertices(self.F, self.b) 92 | if verbose == True: print("vertices: ", self.vertices) 93 | 94 | def computeHRep(self): 95 | self.F, self.b = pypoman.duality.compute_polytope_halfspaces(self.vertices) 96 | 97 | def preAB(self, A, B, Fu, bu): 98 | n = A.shape[1] 99 | d = B.shape[1] 100 | 101 | # Original polytope: 102 | F1 = np.hstack( ( np.dot(self.F, A), np.dot(self.F, B) ) ) 103 | b1 = self.b 104 | 105 | F2 = np.hstack( ( np.zeros((Fu.shape[0], n)), Fu ) ) 106 | b2 = bu 107 | ineq = (np.vstack((F1, F2)), np.hstack(( b1, b2 )) ) # A * x + Bu <= b, F_u u <= bu 108 | 109 | # Projection is proj(x) = [x_0 x_1] 110 | E = np.zeros(( n, n+d )) 111 | E[0:n,0:n] = np.eye(n) 112 | f = np.zeros(n) 113 | proj = (E, f) # proj(x) = E * x + f 114 | 115 | vertices = pypoman.project_polytope(proj, ineq)#, eq=None, method='bretl') 116 | F, b = pypoman.duality.compute_polytope_halfspaces(vertices) 117 | return F, b 118 | 119 | def plot2DPolytope(self, color, linestyle='-o', label = None): 120 | # This works only in 2D!!!! 121 | vertices = pypoman.polygon.compute_polygon_hull(self.F, self.b) 122 | vertices.append(vertices[0]) 123 | xs, ys = zip(*vertices) #create lists of x and y values 124 | plt.plot(xs, ys, linestyle, color=color, label=label) -------------------------------------------------------------------------------- /utils/simulators.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class SIMULATOR(object): 4 | """ Finite Time Optimal Control Problem (FTOCP) 5 | Methods: 6 | - solve: solves the FTOCP given the initial condition x0, terminal contraints (optinal) and terminal cost (optional) 7 | - model: given x_t and u_t computes x_{t+1} = Ax_t + Bu_t 8 | 9 | """ 10 | def __init__(self, system, A = [], B = [], radius = [], dt = 0.1, map = []): 11 | # Define variables 12 | self.system = system 13 | self.A = A 14 | self.B = B 15 | self.radius = radius 16 | self.dt = dt 17 | self.map = map 18 | 19 | def sim(self, x, u): 20 | if self.system == "linear_system": 21 | x_next = self.linear_system(x,u) 22 | elif self.system == "unicycle": 23 | x_next = self.unicycle(x,u) 24 | elif self.system == "dyn_bicycle_model": 25 | x, x_glob = self.dyn_bicycle_model(x, u) 26 | x_next = [x, x_glob] 27 | return x_next 28 | 29 | def linear_system(self, x,u): 30 | return (np.dot(self.A,x) + np.squeeze(np.dot(self.B,u))).tolist() 31 | 32 | def unicycle(self, x, u): 33 | # Given a state x and input u it return the successor state 34 | xNext = np.array([x[0] + self.dt * x[2]*np.cos(u[0] - x[0]/self.radius) / (1 - x[1] / self.radius), 35 | x[1] + self.dt * x[2]*np.sin(u[0] - x[0]/self.radius), 36 | x[2] + self.dt * u[1]]) 37 | return xNext.tolist() 38 | 39 | 40 | # Introduce function for computing road edges 41 | def computeRoadEdges(self, s_start, s_end, circleRadius, roadHalfWidth, signEdge = 1, disc = 1): 42 | edges = [] 43 | for k in np.arange(s_start, s_end+disc, disc):#in range(s_start*disc, s_end*disc): 44 | angle = k/circleRadius 45 | radius = circleRadius + signEdge * roadHalfWidth 46 | edges.append([radius*np.sin(angle), circleRadius-radius*np.cos(angle)]) 47 | 48 | return np.array(edges) 49 | 50 | # Introduce function for change of coordinates from curvilinear absicssa to XY 51 | def from_curvilinear_to_xy(self, xcl_feasible): 52 | feasibleTraj = [] 53 | for k in range(0, np.shape(np.array(xcl_feasible))[0]): 54 | angle = np.array(xcl_feasible)[k, 0]/self.radius 55 | radius_curr = self.radius - np.array(xcl_feasible)[k, 1] 56 | feasibleTraj.append([radius_curr*np.sin(angle), self.radius-radius_curr*np.cos(angle)]) 57 | 58 | return feasibleTraj 59 | 60 | 61 | def dyn_bicycle_model(self, x_states_list, u): 62 | # This method computes the system evolution. Note that the discretization is deltaT and therefore is needed that 63 | # dt <= deltaT and ( dt / deltaT) = integer value 64 | x = np.array(x_states_list[0]) 65 | x_glob = np.array(x_states_list[1]) 66 | 67 | # Vehicle Parameters 68 | m = 1.98 69 | lf = 0.125 70 | lr = 0.125 71 | Iz = 0.024 72 | Df = 0.8 * m * 9.81 / 2.0 73 | Cf = 1.25 74 | Bf = 1.0 75 | Dr = 0.8 * m * 9.81 / 2.0 76 | Cr = 1.25 77 | Br = 1.0 78 | 79 | # Discretization Parameters 80 | deltaT = 0.001 81 | x_next = np.zeros(x.shape[0]) 82 | cur_x_next = np.zeros(x.shape[0]) 83 | 84 | # Extract the value of the states 85 | delta = u[0] 86 | a = u[1] 87 | 88 | psi = x_glob[3] 89 | X = x_glob[4] 90 | Y = x_glob[5] 91 | 92 | vx = x[0] 93 | vy = x[1] 94 | wz = x[2] 95 | epsi = x[3] 96 | s = x[4] 97 | ey = x[5] 98 | 99 | # Initialize counter 100 | i = 0 101 | while( (i+1) * deltaT <= self.dt): 102 | # Compute tire split angle 103 | alpha_f = delta - np.arctan2( vy + lf * wz, vx ) 104 | alpha_r = - np.arctan2( vy - lf * wz , vx) 105 | 106 | # Compute lateral force at front and rear tire 107 | Fyf = Df * np.sin( Cf * np.arctan(Bf * alpha_f ) ) 108 | Fyr = Dr * np.sin( Cr * np.arctan(Br * alpha_r ) ) 109 | 110 | # Propagate the dynamics of deltaT 111 | x_next[0] = vx + deltaT * (a - 1 / m * Fyf * np.sin(delta) + wz*vy) 112 | x_next[1] = vy + deltaT * (1 / m * (Fyf * np.cos(delta) + Fyr) - wz * vx) 113 | x_next[2] = wz + deltaT * (1 / Iz *(lf * Fyf * np.cos(delta) - lr * Fyr) ) 114 | x_next[3] = psi + deltaT * (wz) 115 | x_next[4] = X + deltaT * ((vx * np.cos(psi) - vy * np.sin(psi))) 116 | x_next[5] = Y + deltaT * (vx * np.sin(psi) + vy * np.cos(psi)) 117 | 118 | cur = self.map.curvature(s) 119 | cur_x_next[0] = vx + deltaT * (a - 1 / m * Fyf * np.sin(delta) + wz*vy) 120 | cur_x_next[1] = vy + deltaT * (1 / m * (Fyf * np.cos(delta) + Fyr) - wz * vx) 121 | cur_x_next[2] = wz + deltaT * (1 / Iz *(lf * Fyf * np.cos(delta) - lr * Fyr) ) 122 | cur_x_next[3] = epsi + deltaT * ( wz - (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) * cur ) 123 | cur_x_next[4] = s + deltaT * ( (vx * np.cos(epsi) - vy * np.sin(epsi)) / (1 - cur * ey) ) 124 | cur_x_next[5] = ey + deltaT * (vx * np.sin(epsi) + vy * np.cos(epsi)) 125 | 126 | # Update the value of the states 127 | psi = x_next[3] 128 | X = x_next[4] 129 | Y = x_next[5] 130 | 131 | vx = cur_x_next[0] 132 | vy = cur_x_next[1] 133 | wz = cur_x_next[2] 134 | epsi = cur_x_next[3] 135 | s = cur_x_next[4] 136 | ey = cur_x_next[5] 137 | 138 | # Increment counter 139 | i = i+1 140 | 141 | # Noises 142 | noise_vx = np.max([-0.05, np.min([np.random.randn() * 0.01, 0.05])]) 143 | noise_vy = np.max([-0.05, np.min([np.random.randn() * 0.01, 0.05])]) 144 | noise_wz = np.max([-0.05, np.min([np.random.randn() * 0.005, 0.05])]) 145 | 146 | cur_x_next[0] = cur_x_next[0] + 0.01*noise_vx 147 | cur_x_next[1] = cur_x_next[1] + 0.01*noise_vy 148 | cur_x_next[2] = cur_x_next[2] + 0.01*noise_wz 149 | 150 | x_next[0] = x_next[0] + 0.01*noise_vx 151 | x_next[1] = x_next[1] + 0.01*noise_vy 152 | x_next[2] = x_next[2] + 0.01*noise_wz 153 | 154 | return cur_x_next.tolist(), x_next.tolist() 155 | 156 | 157 | -------------------------------------------------------------------------------- /utils/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy 3 | 4 | class system(object): 5 | """docstring for system""" 6 | def __init__(self, A, B, w_inf, x0): 7 | self.A = A 8 | self.B = B 9 | self.w_inf = w_inf 10 | self.x = [x0] 11 | self.u = [] 12 | self.w = [] 13 | self.x0 = x0 14 | 15 | self.w_v = w_inf*(2*((np.arange(2**A.shape[1])[:,None] & (1 << np.arange(A.shape[1]))) > 0) - 1) 16 | print("Disturbance vertices \n", self.w_v ) 17 | 18 | def applyInput(self, ut): 19 | self.u.append(ut) 20 | self.w.append(np.random.uniform(-self.w_inf,self.w_inf,self.A.shape[1])) 21 | xnext = np.dot(self.A,self.x[-1]) + np.dot(self.B,self.u[-1]) + self.w[-1] 22 | self.x.append(xnext) 23 | 24 | def reset_IC(self): 25 | self.x = [self.x0] 26 | self.u = [] 27 | self.w = [] 28 | 29 | def dlqr(A, B, Q, R, verbose = False): 30 | # solve the ricatti equation 31 | P = np.matrix(scipy.linalg.solve_discrete_are(A, B, Q, R)) 32 | # compute the LQR gain 33 | K = np.array(scipy.linalg.inv(B.T*P*B+R)*(B.T*P*A)) 34 | Acl = A - np.dot(B, K) 35 | 36 | if verbose == True: 37 | print("P: ", P) 38 | print("K: ", K) 39 | print("Acl: ", Acl) 40 | return P, K, Acl --------------------------------------------------------------------------------