MPC_IAC_120622.ipynb

{"cells":[{"cell_type":"code","execution_count":2,"metadata":{"id":"h2Edv8NdfWw4"},"outputs":[],"source":["import numpy as np\n","from scipy.linalg import block_diag\n","import sympy as sym\n","import casadi as cas\n","from casadi import *"]},{"cell_type":"markdown","metadata":{"id":"LPqHLvfMnB9L"},"source":["The Dugoff model for a typical tire is Dugoff 1970, Wong, 1978\n","\n","$$\n","\\begin{aligned}\n","&F_x=-\\frac{C_s s}{1-s} f(\\lambda) \\\\\n","&F_y=-\\frac{C_\\alpha \\tan \\alpha}{1-s} f(\\lambda)\n","\\end{aligned}\n","$$\n","\\\\\n","$$\n","\\begin{aligned}\n","&\\lambda=\\frac{\\mu F_z\\left(1-\\epsilon v \\sqrt{s^2+\\tan ^2 \\alpha}\\right)(1-s)}{2 \\overline{c_s^2 s^2+C_\\alpha^2 \\tan ^2 \\alpha}} \\\\\n","\\\\\n","&f(\\lambda)= \\begin{cases}X(2-\\lambda) & \\text { for } \\lambda<1 \\\\\n","1 & \\text { for } \\lambda>1\\end{cases}\n","\\end{aligned}\n","$$\n","\n","$F_x=$ longitudinal force \\\\\n","$F_y=$ side force\n","\n","$\\mathrm{s}=$ longitudinal slip \\\\\n","$\\alpha=$ tire slip angle \\\\\n","$C_s=$ cornering stiffness \\\\\n","$C_\\alpha=$ longitudinal stiffens \\\\\n","$F_z=$ vertical load \\\\\n","$v=$ vehicle speed \\\\\n","$\\mu=$ coefficient of road adhesion \\\\\n","$\\epsilon=$ adhesion reduction coefficient\n","\n","In this section, we use $\\alpha$ to denote the tire slip angle\n","\n","\n","============================================================================\n","\n","MODEL USED\n","\n","given by (Guntur and Sankar, 1980, Dugoff, 1969)\n","$$\n","F_x=C_\\sigma \\frac{\\sigma_x}{1+\\sigma_x} f(\\lambda)\n","$$\n","and the lateral tire force is given by $\\quad F_y=C_\\alpha \\frac{\\tan (\\alpha)}{1+\\sigma_x} f(\\lambda)$\n","where $\\lambda$ is given by\n","$$\n","\\lambda=\\frac{\\mu F_z\\left(1+\\sigma_x\\right)}{2\\left\\{\\left(C_\\sigma \\sigma_x\\right)^2+\\left(C_\\alpha \\tan (\\alpha)\\right)^2\\right\\}^{1 / 2}}\n","$$\n","and\n","$f(\\lambda)=(2-\\lambda) \\lambda$ if $\\lambda<1$\n","$f(\\lambda)=1$ if $\\lambda \\geq 1$\n","$F_z$ is the vertical force on the tire while $\\mu$ is the tire-road friction coefficient."]},{"cell_type":"code","execution_count":7,"metadata":{"id":"UwOKi2JsfWw6"},"outputs":[],"source":["def DugoffLinearizer(slip_angle,slip_ratio, ca, cs, mu, Fz):\n","  lamb_bar = mu*Fz*(1+slip_ratio)/(2*((cs*slip_ratio)**2+(ca*np.tan(slip_angle))**2)**0.5)\n","  \n","  if slip_angle !=0 or slip_ratio !=0:\n","    if lamb_bar < 1: \n","      fbar = lamb_bar*(2-lamb_bar)\n","      c_bar = (Fz*ca*mu*(np.tan(slip_angle)**2 + 1))/(2*(ca**2*np.tan(slip_angle)**2 + cs**2*slip_ratio**2)**(1/2)) - (Fz*ca**3*mu*np.tan(slip_angle)**2*(np.tan(slip_angle)**2 + 1))/(2*(ca**2*np.tan(slip_angle)**2 + cs**2*slip_ratio**2)**(3/2))\n","\n","      \n","    else:\n","      fbar = 1\n","      c_bar = (ca*(np.tan(slip_angle)**2 + 1))/(slip_ratio + 1)\n","    Fy_bar= ca*np.tan(slip_angle)*fbar/(1+slip_ratio)\n","    c_bar = c_bar.item()\n","    Fy_bar = Fy_bar.item()\n","                                                                 \n","  else:\n","    fbar = 1\n","    c_bar = ca.item()\n","    Fy_bar = 0\n","\n","  return Fy_bar, c_bar, slip_angle"]},{"cell_type":"markdown","metadata":{"id":"w9TB0Ct1nKPk"},"source":["**Optimal_Control_partI.pdf**\n","2. Linear Quadratic Optimal Control\n","2.1 Batch Approach Solution\n","\n","Starting with $x_0=x(0)$, we have $x_1=A x(0)+B u_0$, and $x_2=A x_1+B u_1=A^2 x(0)+A B u_0+B u_1$, by substitution for $x_1$, and so on. Continuing up to $x_N$ we obtain:\n","$$\n","\\left[\\begin{array}{c}\n","x_0 \\\\\n","x_1 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","x_N\n","\\end{array}\\right]=\\left[\\begin{array}{c}\n","I \\\\\n","A \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","A^N\n","\\end{array}\\right] x(0)+\\left[\\begin{array}{cccc}\n","0 & \\cdots & \\cdots & 0 \\\\\n","B & 0 & \\cdots & 0 \\\\\n","A B & B & \\cdots & 0 \\\\\n","\\vdots & \\ddots & \\ddots & 0 \\\\\n","A^{N-1} B & \\cdots & A B & B\n","\\end{array}\\right]\\left[\\begin{array}{c}\n","u_0 \\\\\n","u_1 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","u_{N-1}\n","\\end{array}\\right]\n","$$\n","\n","The equation above can be represented as\n","$$\n","\\mathcal{X} \\triangleq \\mathcal{S}^x x(0)+\\mathcal{S}^u U_0\n","$$"]},{"cell_type":"code","execution_count":4,"metadata":{"id":"V8DW2LhGfWw6"},"outputs":[],"source":["def Sx_Su_Sw_Sd_Sf(A, B, E, D, F, N):\n","  nX = np.size(A,0)\n","  nU = np.size(B,1)\n","  nE = np.size(E,1)\n","  \n","  Sx = np.eye(nX)\n","  A_tmp = A\n","  for i in range(N):\n","      Sx = np.vstack((Sx, A_tmp))\n","      A_tmp = A_tmp @ A\n","  \n","  SxB = Sx @ B\n","  Su = np.zeros((nX*(N+1),nU*N))\n","  for j in range(N):\n","      Su_tmp = np.vstack((np.zeros((nX, nU)), SxB[:-nX,:]))\n","      Su[:, 8*j:8*j+nU] = Su_tmp.reshape(Su_tmp.shape[0], nU)\n","      SxB = Su_tmp\n","  \n","  SxE = Sx @ E\n","  Sw = np.zeros((nX*(N+1),nE*N))\n","  for k in range(N):\n","      Sw_tmp = np.vstack((np.zeros((nX, nE)), SxE[:-nX,:]))\n","      Sw[:, 8*k:8*k+nE] = Sw_tmp.reshape(Sw_tmp.shape[0], nE)\n","      SxE = Sw_tmp\n","  \n","  Sd = np.zeros((nX,1)).reshape((nX,1))\n","  D_tmp = D.reshape(nX,1)\n","  for i in range(N):\n","      Sd = np.vstack((Sd, D_tmp))\n","      D_tmp = A @ D_tmp + D\n","      \n","  Sf = np.zeros((nX,1)).reshape((nX,1))\n","  F_tmp = F.reshape(nX,1)\n","  for i in range(N):\n","      Sf = np.vstack((Sf, F_tmp))\n","      F_tmp = A @ F_tmp + F\n","\n","  return Sx, Su, Sw, Sd, Sf"]},{"cell_type":"code","execution_count":15,"metadata":{"id":"FDhGJUSZfWw7"},"outputs":[],"source":["# States: y,v,psi, r, phi, phi_dot\n","# Inputs: Change in each wheel torque and in each wheel steer angle\n","def CFTOC_VD(slip_angle, slip_ratio, ca, cs, mux,muy, fz0,fy0, u,reff,xd,tf,tr,lf,lr,ms,hs,m,ixx,ks,ls,g,bs,iz,kus_d,Ts,Q,R,W0,Phir,alphamax,Qmax ,Qmin, deltamax,x0,yd,N):\n","  \"\"\"\n","  STATES:\n","  y: lateral position\n","  vy: lateral speed\n","  psi: heading angle\n","  r: yaw rate\n","  phi: roll angle\n","  phid: roll rate\n","  \n","  =================================================================\n","  =================================================================\n","  Input Definitions:\n","\n","  slip_angle: tire slip angle at each tire (aka alpha) --- (5x1)\n","  slip_ratio: tire slip ratio at each tire (aka sigma) --- (5x1)\n","  ca: tire cornering stifness (aka C_alpha)            --- (5x1)\n","  cs: tire longitudinal stiffness (aka C_sigma)        --- (5x1)\n","  mux: coefficient of friction in x                    --- (scalar)\n","  muy: coefficient of friction in y                    --- (scalar)\n","  fz0: tire vertical load                              --- (5x1)\n","  fyo: tire lateral load                               --- (5x1)\n","  u: longitudinal velcoity                             --- (scalar)\n","  reff: tire effective radius of rotation              --- (scalar)\n","  delta1: initial commanded steering input             --- (scalar)\n","  tf: front wheel track (dist between tire centers)    --- (scalar)\n","  tr: rear wheel track (dist between tire centers)     --- (scalar)\n","  lf: distance from cg to front wheels                 --- (scalar)\n","  lr: distance from cg to rear wheels                  --- (scalar)\n","  ms: vehicle sprung mass (without mass of tires and suspension) --- (scalar)\n","  hs: cg of sprung mass to roll center                 --- (scalar)\n","  m: total mass of vehicle (sprung and unsprung)       --- (scalar)\n","  ixx: moment of inertia about the x-axis (roll)       --- (scalar)\n","  ks: spring stiffness of suspension                   --- (scalar)\n","  ls: distance between suspension fixtures to vehicle  --- (scalar)\n","  g: gravity                                           --- (scalar)\n","  bs: suspension damping coefficient                   --- (scalar)\n","  iz: vehivle moment of inertia about z axis (yaw)     --- (scalar)\n","  kus_d: desired understeer coefficient of vehicle     --- (scalar)\n","  rmax: maximum allowed yaw rate (to be selected)      --- (scalar)\n","  Ts: sampling time for euler discretization           --- (scalar)\n","  Q: cost matrix for states                            --- (5x5)\n","  R: cost matrix for inputs                            --- (5x5)\n","  W0: initial torques and steering angles [Q1,delta1,Q2,delta2,Q3,delta3,Q4,delta4] ---(8x1)\n","  Phir: banking angle at over the prediction horizon   --- (Nx1)\n","  alphamax: maximum allowable rear wheel side slip     --- (scalar)\n","  Qmax: maximum torque attainable at each wheel        --- (scalar)\n","  deltamax: maximum steering angle                     --- (scalar)\n","  x0: initial states [y,vy,r,phi,phid]                 --- (5x1)\n","  psid: desired heading angle                          --- (scalar)\n","  yd:desired lateral position                          --- (scalar)\n","  N: MPC Prediction horizon                            --- (scalar)\n","  \"\"\"\n","  psi_desired = np.arctan(yd/xd)\n","  if u != 0:\n","    delta1 = psi_desired + x0[1]/u\n","  else: \n","    delta1 = psi_desired\n","  # Tire model Linearization \n","  fy1, ca1, a1 = DugoffLinearizer(slip_angle[0],slip_ratio[0], ca[0], cs[0], mux, fz0[0])\n","  fy2, ca2, a2 = DugoffLinearizer(slip_angle[1],slip_ratio[1], ca[1], cs[1], mux, fz0[1])\n","  fy3, ca3, a3 = DugoffLinearizer(slip_angle[2],slip_ratio[2], ca[2], cs[2], mux, fz0[2])\n","  fy4, ca4, a4 = DugoffLinearizer(slip_angle[3],slip_ratio[3], ca[3], cs[3], mux, fz0[3])  \n","  B11 = np.array([[0,0,0,0,0], [0,-ca1/u, -lf*ca1/u, 0, 0]])\n","  B21 = np.array([[1/reff, 0],[0, ca1]])\n","  D11 = np.array ([0, fy1-ca1*a1])\n","\n","  B12 = np.array([[0,0,0,0,0], [0,-ca2/u, -lf*ca2/u, 0, 0]])\n","  B22 = np.array([[1/reff, 0],[0, ca2]])\n","  D12 = np.array ([0, fy2-ca2*a2])\n","\n","  B13 = np.array([[0,0,0,0,0], [0,-ca3/u, lr*ca3/u, 0, 0]])\n","  B23 = np.array([[1/reff, 0],[0, ca3]])\n","  D13 = np.array ([0, fy3-ca3*a3])\n","\n","  B14 = np.array([[0,0,0,0,0], [0,-ca4/u, lr*ca4/u, 0, 0]])\n","  B24 = np.array([[1/reff, 0],[0, ca4]])\n","  D14 = np.array ([0, fy4-ca4*a4])\n","  \n","  #Local actuator reconfiguration matrix: 0 inactive actuator 1 active actuator\n","  tq1 = 0\n","  tq2 = 0\n","  tq3 = 0\n","  tq4 = 0\n","  tdelta1 = 1\n","  tdelta2 = 1\n","  tdelta3 = 0\n","  tdelta4 = 0\n","\n","  Tw1 = np.array([[tq1,0],[0,tdelta1]])\n","  Tw2 = np.array([[tq2,0],[0,tdelta2]])\n","  Tw3 = np.array([[tq3,0],[0,tdelta3]])\n","  Tw4 = np.array([[tq4,0],[0,tdelta4]])\n","\n","  #Reconfiguration (Mapping) Matricies: Delta1 input\n","  Lw1 = np.array ([[np.cos(delta1), -np.sin(delta1)],[np.sin(delta1), np.cos(delta1)]])\n","  Lw2 = np.array ([[np.cos(delta1), -np.sin(delta1)],[np.sin(delta1), np.cos(delta1)]])\n","  Lw3 = np.array([[1,0],[0,1]])\n","  Lw4 = np.array([[1,0],[0,1]])\n","\n","  #Mapping Matrix from corner forces\n","  Lc = np.array([[1,0,1,0,1,0,1,0],[0,1,0,1,0,1,0,1], [-tf/2,lf,tf/2,lf,-tr/2,-lr,tr/2,-lr]])\n","\n","  # Vehicle Body Dynamics y vy psi,psidot roll, rollrate \n","  # Equations written in block 1 below\n","  den =2*(ms**2*hs**2+m*ms*hs**2+m*ixx)\n","  Af = np.array([[0,1,0,0,0], [0,0,-u, (hs*ks*ls**2*ms-2*g*hs**2*ms**2)/den, bs*hs*ls**2*ms/den],[0,0,0,0,0],[0,0,0,0,1],[0,0,0,(2*g*hs*m*ms-ks*ls**2*m)/den, -bs*ls*ls*m/den]])\n","  Bf = np.array([[0,0,0],[0,(ixx+ms*hs**2)/den, 0],[0,0,1/iz],[0,0,0],[0,2*ms*hs/den,0]])\n","  Cpr = np.array([[0],[(2*g*ms**2*hs**2-hs*ks*ls**2*ms)/den],[0],[0],[(ks*ls**2*m-2*g*hs*m*ms)/den]])\n","\n","  # X_dot = AX+EW+BU+D+FPhir\n","  # W = wheel torque, steer angle for each wheel (8x1)\n","  # States are y v r phi, phi_dot (ignoring tyre dynamics)\n","  # U = change in wheel torque, change in steering angle for each tire\n","\n","  Tw = block_diag(Tw1,Tw2,Tw3,Tw4)\n","  Lw = block_diag(Lw1, Lw2, Lw3, Lw4)\n","  B1 = np.hstack((B11.T, B12.T, B13.T, B14.T)).T\n","  B2 = block_diag(B21, B22, B23, B24)\n","  D1 = np.hstack((D11,D12,D13,D14)).T\n","  \n","  A = Af+Bf@Lc@Lw@B1\n","  E = Bf@Lc@Lw@B2\n","  B = Bf@Lc@Lw@B2@Tw\n","  D = Bf@Lc@Lw@D1\n","  D = D.reshape(5,1)\n","  F = Cpr\n","  print(D)\n","###############################################################################\n","  ## CONTROL OBJECTIVES ##\n","  rmax = muy*g/u\n","  l= lf+lr\n","  rb = u/(l+kus_d*u**2)*delta1\n","\n","  rd = np.sign(delta1)*np.min([np.abs(rb), rmax])\n","  if isinstance(rd, float):\n","    rd = rd\n","  else: \n","    rd =rd.item()\n","    \n","  xd = np.array([yd,0, rd, 0 ,0])\n","  xd = xd.reshape(5,1)\n","  nX = 5\n","  nU = 8\n","  nW = 8\n","  nphir = 1\n","  # In the paper they use zero order hold, I will use euler discretization\n","  Ad = Ts*A + np.eye(np.size(A,0))\n","  Bd = Ts*B\n","  Ed = Ts*E\n","  Dd = Ts*D\n","  Fd = Ts*F\n","\n","  #Formulating Problem:\n","  Sx, Su, Sw, Sd, Sf = Sx_Su_Sw_Sd_Sf(Ad, Bd, Ed, Dd, Fd, N) ## Check blocks 2 and 3 below for explanation\n","  # We need a weighting matrix Q. I would say emphasis on tracking lateral position, then heading, then lateral velocity, with little concern for roll charact.\n","  PN = Q\n","  Q_temp = np.kron(np.eye(N),Q)\n","  Qbar  = block_diag(Q_temp,PN)\n","  ##R = np.array(np.ones((8,1)))\n","  ##R_temp = np.kron(np.eye(N), np.diag(R))\n","  Rbar = 0.5* np.eye(nU*N)\n","  ##Rbar = block_diag(R_temp)\n","\n","  Wbar = np.kron(np.ones((1,N)), W0).T #W0 recieved from vehicle, vector of inputs at time zero #### check\n","  Xdbar = np.kron(np.ones((1,N+1)), xd.T).T\n","  # Check block 4\n","  H = Su.T@Qbar@Su + Rbar\n","  F1 = Sx.T @Qbar @ Su\n","  F2 = Sw.T @ Qbar @ Su\n","  F3 = Sd.T@Qbar@Su\n","  F4 = - Qbar@Su\n","  F5 = Sf.T@Qbar@Su\n","\n","  # Finally Cost Matricies:\n","  P = H\n","  x0 = x0.reshape(5,1)\n","  q = x0.T@F1+Wbar.T@F2 +F3+ Phir*F5+ Xdbar.T@F4 #x0 from vehicle, xdes from path planner\n","\n","   \n","  \n","  Ax= np.array([[0,0,1,0,0],[0,0,-1,0,0],[0,-1/u,lr/u,0,0],[0,1/u,-lr/u,0,0]])\n","  bx = np.array([rmax, rmax, alphamax,alphamax]).reshape(4,1)\n","  Af = np.array([[0,0,1,0,0],[0,0,-1,0,0],[0,-1/u,lr/u,0,0],[0,1/u,-lr/u,0,0],[1,0,0,0,0],[-1,0,0,0,0]])\n","  bf = np.array([rmax, rmax, alphamax,alphamax,yd,-yd]).reshape(6,1)\n","\n","\n","  Au2 = np.array([[0,1,0,0,0,0,0,0],[0,0,0,1,0,0,0,0],[0,0,0,0,0,1,0,0],[0,0,0,0,0,0,0,1]])\n","  Au3 = np.array([[0,1,0,-1,0,0,0,0],[0,-1,0,1,0,0,0,0]])\n","  Au = np.vstack((Au2,-Au2,Au3))\n","\n","  bufip = deltamax*np.array([1,1,1,1])-np.array([W0[1],W0[3],W0[5],W0[7]])\n","  busip = deltamax*np.array([1,1,1,1])+np.array([W0[1],W0[3],W0[5],W0[7]])\n","  bufip = bufip.reshape(4,1)\n","  busip = busip.reshape(4,1)\n","  bu = np.vstack((bufip,busip,0.0,0.0))\n","\n","  # Check Block 5\n","  G0_firstpart= block_diag(np.kron(np.eye(N),Au))\n","  temp = block_diag(np.kron(np.eye(N),Ax),Af)\n","  G0_sp = temp@Su\n","  W_contrib = temp@Sw@Wbar\n","  D_contrib = temp@Sd\n","  Phir_cont = temp@Sf*Phir\n","  G0 = np.vstack((G0_firstpart,G0_sp))\n","  w0_2 = np.vstack((np.kron(np.ones((N,1)),bx), bf))\n","  w0_2 = w0_2-W_contrib-D_contrib-Phir_cont\n","  w0 = np.vstack((np.kron(np.ones((N,1)),bu), w0_2))\n","  e0_sp = -Ax\n","  Ax_tmp = -Ax@Ad\n","  Ax_tent = -Ad\n","  for i in range(N-1):\n","      e0_sp = np.vstack((e0_sp, Ax_tmp))\n","      Ax_tmp = Ax_tmp @ Ad\n","      Ax_tent = Ax_tent @Ad\n","  e0_sp = np.vstack((e0_sp, Af@Ax_tent))\n","  e0_fp = np.zeros((N*np.shape(bu)[0], 5)) ##2 length of Bu *n\n","  E0 = np.vstack((e0_fp,e0_sp))\n","  A = G0\n","  b = E0@x0+w0\n","  b = b.reshape(np.shape(b)[0],)\n","\n","  # Optimizing with CasADI\n","  qp ={}\n","  H  = 2*cas.DM(H)\n","  #g = cas.DM(q)\n","\n","  uba = cas.DM(b.astype(float))\n","  A = cas.DM(A.astype(float))\n","  A = A.reshape((A.size1(),H.size1()))\n","\n","  qp ={}\n","  qp['h']= H.sparsity()\n","  qp['a']=A.sparsity()\n","  S = cas.conic('S','osqp',qp)\n","\n","  r = S(h = H, g = q,\\\n","        a =A, uba = uba)\n","  uOpt = r['x']\n","  JOpt = r['cost']\n","  x_opt = Sx@x0+Su@uOpt+Sw@Wbar+Sd+Sf*Phir \n","  return x_opt,uOpt, JOpt"]},{"cell_type":"markdown","metadata":{"id":"SRpfKWuwfWw8"},"source":["## Block 1: Vehicle Dynamics Equations (Derived)\n","Assumptions: the roll angle and bank angle difference is small\n","\n","\\begin{aligned}\n","\\frac{d}{dt}\\begin{bmatrix}\n","y \\\\\n","v_y \\\\\n","\\psi \\\\\n","r \\\\ \n","\\phi \\\\\n","\\dot{\\phi}\\\\\n","\\end{bmatrix} = \n","\\begin{bmatrix}\n","0 & 1 & 0 & 0 & 0 & 0\\\\\n","0 & 0 & 0 & -u & \\frac{h_s k_s l_s^2 m_s -2g h_s^2 m_s^2}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)} &\\frac{b_s h_s l_s^2 m_s}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)}\\\\\n","0 & 0 & 0 &1 & 0 & 0\\\\\n","0 & 0 & 0 & 0 & 0 & 0 \\\\\n","0 & 0 & 0 & 0 & 0 & 0 \\\\ \n","0 & 0 & 0 & 0 & \\frac{2 g h_s m m_s - k_s l_s^2 m}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)} & \\frac{-b_s l_s^2 m}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)}\n","\\end{bmatrix}\n","\\begin{bmatrix}\n","y \\\\\n","v_y \\\\\n","\\psi \\\\\n","r \\\\ \n","\\phi \\\\\n","\\dot{\\phi}\\\\\n","\\end{bmatrix} +\\begin{bmatrix}\n","0 & 0 &0\\\\\n","0 & \\frac{I_xx+m_s h_s^2}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)} & 0\\\\\n","0 & 0 & 0 \\\\\n","0 & 0 & \\frac{1}{I_z}\\\\\n","0 & 0 & 0\\\\\n","0 & \\frac{2 m_s h_s}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)} & 0\n","\\end{bmatrix}\n","\\begin{bmatrix}\n","F_x \\\\\n","F_y \\\\\n","M_z\n","\\end{bmatrix} + \\begin{bmatrix}\n","0 \\\\\n","\\frac{2 g m_s^2 h_s^2 - h_s k_s l_s^2 m_s}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)} \\\\\n","0 \\\\\n","0 \\\\\n","0 \\\\\n","\\frac{k_s l_s^2 m - 2 g h_s m m_s}{2(m_s^2 h_s^2 - h_s k_s l_s^2 m_s)}\n","\\end{bmatrix} \\phi_r \n","\\end{aligned} \\\\\n","\n","\\begin{aligned}\n","\\dot{X_b}= A_F X_b + B_F F +C_\\phi \\phi_r\n","\\end{aligned}\n","\n","\n","\n","With substitution of the matrices required for mapping we get the following:\n","\n","$$\n","\\dot{X}_b=\\left(A_F+B_F L_c L_w B_1\\right) X_b+\\left(B_F L_c L_w B_2\\right) W+\\left(B_F L_c L_w B_2 T_w\\right) U+B_F L_c L_w D_1 +C_\\phi \\phi_r\n","$$\n","\n","$$\n","\\begin{aligned}\n","&W=\\left[Q_1 \\delta_1 Q_2 \\delta_2 Q_3 \\delta_3 Q_4 \\delta_4\\right]^T \\\\\n","&U=\\left[\\Delta Q_1 \\Delta \\delta_1 \\Delta Q_2 \\Delta \\delta_2 \\Delta Q_3 \\Delta \\delta_3 \\Delta Q_4 \\Delta \\delta_4\\right]^T \\\\\n","&\n","\\end{aligned}\n","$$"]},{"cell_type":"markdown","metadata":{"id":"IAl4WCG2fWw9"},"source":["## Block 2: In QP format, the problem has the following formulation\n","$$\n","\\begin{aligned}\n","&{\\left[\\begin{array}{c}\n","X(0) \\\\\n","X^1 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","X^N\n","\\end{array}\\right]=\\left[\\begin{array}{c}\n","I \\\\\n","A_d \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","A_d^N\n","\\end{array}\\right] X(0)+\\left[\\begin{array}{cccc}\n","0 & \\ldots & 0 \\\\\n","B_d & 0 & \\ldots & 0 \\\\\n","A_d B_d & \\ddots & \\ddots & \\vdots \\\\\n","\\vdots & \\ddots & \\ddots & \\vdots \\\\\n","A_d^{N-1} B_d & \\ldots & B_d\n","\\end{array}\\right]\\left[\\begin{array}{c}\n","U^0 \\\\\n","U^1 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","U^{N-1}\n","\\end{array}\\right]} \\\\\n","&+\\left[\\begin{array}{cccc}\n","0 & \\ldots & 0 \\\\\n","E_d & 0 & \\ldots & 0 \\\\\n","A_d E_d & \\ddots & \\ddots & \\vdots \\\\\n","\\vdots & \\ddots & \\ddots & \\vdots \\\\\n","A_d^{N-1} E_d & \\ldots & \\ldots & E_d\n","\\end{array}\\right]\\left[\\begin{array}{c}\n","W^0 \\\\\n","W^0 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","W^0\n","\\end{array}\\right]+\\left[\\begin{array}{c}\n","0 \\\\\n","D_d \\\\\n","A_d D_d \\\\\n","\\vdots \\\\\n","A_d^{N-1} D_d\n","\\end{array}\\right] \\\\\n","&{+\\left[\\begin{array}{cccc}\n","0 & \\ldots & 0 \\\\\n","F_d & 0 & \\ldots & 0 \\\\\n","A_d F_d & \\ddots & \\ddots & \\vdots \\\\\n","\\vdots & \\ddots & \\ddots & \\vdots \\\\\n","A_d^{N-1} F_d & \\ldots & F_d\n","\\end{array}\\right]\\left[\\begin{array}{c}\n","\\phi_r^0 \\\\\n","\\phi_r^1 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","\\phi_r^{N-1}\n","\\end{array}\\right]} \\\\\n","&\n","\\end{aligned}\n","$$"]},{"cell_type":"markdown","metadata":{"id":"m7P8C10EfWw9"},"source":["## Block 3: Where we define the following (as per the function Sx_Su_Sw_Sd_Sf)\n","\n","\\begin{aligned}\n","&\\bar{X}=\\left[\\begin{array}{c}\n","X(0) \\\\\n","X^1 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","X^N\n","\\end{array}\\right], \\quad \\bar{U}=\\left[\\begin{array}{c}\n","U^0 \\\\\n","U^1 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","U^{N-1}\n","\\end{array}\\right], \\quad \\bar{W}=\\left[\\begin{array}{c}\n","W^0 \\\\\n","W^0 \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","W^0\n","\\end{array}\\right] \\\\\n","&S_w=\\left[\\begin{array}{cccc}\n","0 & \\ldots & \\ldots & 0 \\\\\n","E_d & 0 & \\ldots & 0 \\\\\n","A_d E_d & \\ddots & \\ddots & \\vdots \\\\\n","\\vdots & \\ddots & \\ddots & \\vdots \\\\\n","A_d^{N-1} E_d & \\ldots & E_d\n","\\end{array}\\right], \\quad S_d=\\left[\\begin{array}{c}\n","0 \\\\\n","D_d \\\\\n","A_d D_d \\\\\n","\\vdots \\\\\n","A_d^{N-1} D_d\n","\\end{array}\\right] \\\\\n","&\n","\\end{aligned}\n"]},{"cell_type":"markdown","metadata":{"id":"pMvyk09Alzo9"},"source":["\n","\n","\n","$$\n","S_x=\\left[\\begin{array}{c}\n","I \\\\\n","A_d \\\\\n","\\vdots \\\\\n","\\vdots \\\\\n","A_d^N\n","\\end{array}\\right], \\quad S_u=\\left[\\begin{array}{cccc}\n","0 & \\ldots & 0 \\\\\n","B_d & 0 & \\ldots & 0 \\\\\n","A_d B_d & \\ddots & \\ddots & \\vdots \\\\\n","\\vdots & \\ddots & \\ddots & \\vdots \\\\\n","A_d^{N-1} B_d & \\ldots & B_d\n","\\end{array}\\right], \\quad S_f=\\left[\\begin{array}{cccc}\n","0 & \\ldots & 0 \\\\\n","F_d & 0 & \\ldots & 0 \\\\\n","A_d F_d & \\ddots & \\ddots & \\vdots \\\\\n","\\vdots & \\ddots & \\ddots & \\vdots \\\\\n","A_d^{N-1} F_d & \\ldots & F_d\n","\\end{array}\\right]\n","$$\n","\n"]},{"cell_type":"markdown","metadata":{"id":"S76pRQArlxJN"},"source":[]},{"cell_type":"markdown","metadata":{"id":"PDSUCpoAfWw-"},"source":["## Block 4: Cost function\n","$$\n","\\begin{gathered}\n","J=\\frac{1}{2} \\bar{U}^T H \\bar{U}+\\left(X(0)^T F_1+\\bar{W}^T F_2+F_3+\\bar{X}_d^T F_4+\\bar{\\phi}_r^T F_5\\right) \\bar{U}+\\text { Const. } \\\\\n","H=S_u^T \\bar{Q} S_u+\\bar{R} \\\\\n","F_1=S_x^T \\bar{Q} S_u \\\\\n","F_2=S_w^T \\bar{Q} S_u \\\\\n","F_3=S_d^T \\bar{Q} S_u \\\\\n","F_4=-\\bar{Q} S_u\\\\\n","F_5= S_f^T \\bar{Q} S_u\n","\\end{gathered}\n","$$"]},{"cell_type":"markdown","metadata":{"id":"Prd5o3GrfWw-"},"source":["## Block 5: Linear constraint formulation\n","$$\n","\\mathcal{X}=\\left\\{x \\mid A_x x \\leq b_x\\right\\} \\quad \\mathcal{U}=\\left\\{u \\mid A_u u \\leq b_u\\right\\} \\quad \\mathcal{X}_f=\\left\\{x \\mid A_f x \\leq b_f\\right\\}\n","$$\n","Then $G_0, E_0$ and $w_0$ are defined as follows\n","$$\n","G_0=\\left[\\begin{array}{cccc}\n","A_u & 0 & \\cdots & 0 \\\\\n","0 & A_u & \\cdots & 0 \\\\\n","\\vdots & \\vdots & \\ddots & \\vdots \\\\\n","0 & 0 & \\cdots & A_u \\\\\n","0 & 0 & \\cdots & 0 \\\\\n","A_x B & 0 & \\cdots & 0 \\\\\n","A_x A B & A_x B & \\cdots & 0 \\\\\n","\\vdots & \\vdots & \\ddots & \\vdots \\\\\n","A_f A^{N-1} B & A_f A^{N-2} B & \\ldots & A_f B\n","\\end{array}\\right], E_0=\\left[\\begin{array}{c}\n","0 \\\\\n","0 \\\\\n","\\vdots \\\\\n","0 \\\\\n","-A_x \\\\\n","-A_x A \\\\\n","-A_x A^2 \\\\\n","\\vdots \\\\\n","-A_f A^N\n","\\end{array}\\right], w_0=\\left[\\begin{array}{c}\n","b_u \\\\\n","b_u \\\\\n","\\vdots \\\\\n","b_u \\\\\n","b_x \\\\\n","b_x \\\\\n","b_x \\\\\n","\\vdots \\\\\n","b_f\n","\\end{array}\\right]\n","$$"]},{"cell_type":"code","execution_count":18,"metadata":{"id":"7UhRa7aqfWw-","outputId":"1dec6015-f785-4e48-d6f9-7cf9ee940f2f"},"outputs":[{"name":"stdout","output_type":"stream","text":["[[0.]\n"," [0.]\n"," [0.]\n"," [0.]\n"," [0.]]\n"]},{"ename":"TypeError","evalue":"operand type(s) all returned NotImplemented from __array_ufunc__(<ufunc 'add'>, '__call__', array([0, 0, 0, 0, 0, 0, 0, 0]), DM([0, -0.00695156, 0, -0.00695156])): 'ndarray', 'DM'","output_type":"error","traceback":["\u001b[1;31m---------------------------------------------------------------------------\u001b[0m","\u001b[1;31mTypeError\u001b[0m                                 Traceback (most recent call last)","Cell \u001b[1;32mIn [18], line 68\u001b[0m\n\u001b[0;32m     66\u001b[0m x0 \u001b[39m=\u001b[39m xopt[\u001b[39m4\u001b[39m:\u001b[39m8\u001b[39m]\n\u001b[0;32m     67\u001b[0m u0 \u001b[39m=\u001b[39m uopt[\u001b[39m0\u001b[39m:\u001b[39m4\u001b[39m]\n\u001b[1;32m---> 68\u001b[0m W0 \u001b[39m=\u001b[39m W0 \u001b[39m+\u001b[39;49m u0\n\u001b[0;32m     69\u001b[0m slip_angle_1 \u001b[39m=\u001b[39m W0[\u001b[39m1\u001b[39m] \u001b[39m-\u001b[39m (x0[\u001b[39m1\u001b[39m]\u001b[39m+\u001b[39mlf\u001b[39m*\u001b[39mx0[\u001b[39m2\u001b[39m])\u001b[39m/\u001b[39mu\u001b[39m*\u001b[39mnp\u001b[39m.\u001b[39mones((\u001b[39m2\u001b[39m,))\n\u001b[0;32m     70\u001b[0m slip_angle_2 \u001b[39m=\u001b[39m (x0[\u001b[39m1\u001b[39m]\u001b[39m-\u001b[39mlr\u001b[39m*\u001b[39mx0[\u001b[39m2\u001b[39m])\u001b[39m/\u001b[39mu\u001b[39m*\u001b[39mnp\u001b[39m.\u001b[39mones((\u001b[39m2\u001b[39m,))\n","\u001b[1;31mTypeError\u001b[0m: operand type(s) all returned NotImplemented from __array_ufunc__(<ufunc 'add'>, '__call__', array([0, 0, 0, 0, 0, 0, 0, 0]), DM([0, -0.00695156, 0, -0.00695156])): 'ndarray', 'DM'"]}],"source":["# States: y,v,psi, r, phi, phi_dot\n","# Inputs: Change in each wheel torque and in each wheel steer angle\n","\n","# Vehicle Parameters: \n","reff = 0.29\n","lf = 1.6566\n","lr = 1.3152\n","l = lf+lr\n","tf = 1.638762\n","tr = 1.5239686\n","m = 803.182 #kg\n","mf = 782/2.2\n","mr = 985/2.2\n","mufw = 29.5\n","murw = 36\n","ms = m - 2*(mufw+murw)\n","ixx = 200 \n","hs = 0.1 #considered CG and roll center to be same point ##############\n","iy = 800\n","iz = 1200\n","g = 9.81\n","\n","cs = 30000*np.ones((4,)) ####\n","ca = 20000*np.ones((4,)) #####\n","x0 = np.array([0,0,0,0,0]).reshape(5,)\n","slip_angle = np.array([0.0,0,0,0]).reshape(4,)\n","slip_ratio = np.array([0.1,0.1,0.1,0.1]).reshape(4,)\n","fz0 = m*g/4*np.ones((4,))\n","W0 = np.array([0,0,0,0,0,0,0,0])\n","fy0 = np.array([0,0,0,0])\n","kus_d = mf /ca[0] +mr /cs[0] #Caf and Car needed\n","mux = 1\n","muy =1\n","u = 60\n","xd = 100\n","ks = 10000\n","ls =1\n","bs = 1000\n","Ts = 0.05\n","Q =np.eye(5)\n","R = 100* np.eye(8)\n","phir = 0.2\n","alphamax = 0.2\n","Qmax = 1000\n","Qmin = -2000\n","deltamax = 0.1\n","yd = 5\n","N = 100\n","#################################################################################\n","nx = 5\n","nu = 8\n","Q[0,0] = 20\n","Q[2,2]=10\n","\n","M = 60  # Simulation time\n","\n","xOpt = np.zeros((nx, M+1))\n","\n","uOpt = np.zeros((nu, M))\n","\n","xPred = np.zeros((nx, N+1, M))\n","\n","feas = np.zeros((M, ), dtype=bool)\n","for t in range(M):\n","  xopt,uopt,Jopt = CFTOC_VD(slip_angle, slip_ratio, ca, cs, mux,muy, fz0,fy0, u,reff,xd,tf,tr,lf,lr,ms,hs,m,ixx,ks,ls,g,bs,iz,kus_d,Ts,Q,R,W0,phir,alphamax,Qmax,Qmin,deltamax,x0,yd,N)    \n","  x0 = xopt[4:8]\n","  u0 = uopt[0:7]\n","  W0 = W0 + u0\n","  slip_angle_1 = W0[1] - (x0[1]+lf*x0[2])/u*np.ones((2,))\n","  slip_angle_2 = (x0[1]-lr*x0[2])/u*np.ones((2,))\n","  slip_angle = np.hstack((slip_angle_1,slip_angle_2))\n","  slip_ratio = 0.1*np.ones((4,))\n","  W0 = W0 + u0\n","  fz1 = m*g/2 -ks*ls/2*np.sin(x0[3]-phir)-bs*ls/2*x0[4]*np.cos(x0[3]-phir)\n","  fz2 = m*g/2 +ks*ls/2*np.sin(x0[3]-phir)+bs*ls/2*x0[4]*np.cos(x0[3]-phir)\n","  fz3 = m*g/2 -ks*ls/2*np.sin(x0[3]-phir)-bs*ls/2*x0[4]*np.cos(x0[3]-phir)\n","  fz4 = m*g/2 +ks*ls/2*np.sin(x0[3]-phir)+bs*ls/2*x0[4]*np.cos(x0[3]-phir)\n","  fz0 = np.array([fz1,fz2,fz3,fz4])\n","  fy1= ca[0]*(u0[1]- slip_angle_1[0])\n","  fy2= ca[0]*(u0[1]- slip_angle_1[1])\n","  fy3= ca[0]*(- slip_angle_2[0])\n","  fy4= ca[0]*(- slip_angle_2[1])\n","  fy0 = np.array([fy1,fy2,fy3,fy4]).reshape(4,)\n","  yd = yd - x0[0]\n","  x0[0] = 0\n","  xd = xd -u*Ts\n","  \n","  print('yd')\n","  print(yd)\n","  print('Fy0')\n","  print(fy0)\n","  print('Fz0')\n","  print(fz0)\n","  print('slip_angle')\n","  print(slip_angle)\n","  '''\n","  print(slip_angle)\n","  '''"]}],"metadata":{"colab":{"provenance":[]},"kernelspec":{"display_name":"Python 3.9.13 ('MPC_IAC')","language":"python","name":"python3"},"language_info":{"codemirror_mode":{"name":"ipython","version":3},"file_extension":".py","mimetype":"text/x-python","name":"python","nbconvert_exporter":"python","pygments_lexer":"ipython3","version":"3.9.13"},"orig_nbformat":4,"vscode":{"interpreter":{"hash":"db50736e4b802013441bba5f1417c43b8d9747ad1f92335c1361cb983a9a0402"}}},"nbformat":4,"nbformat_minor":0}