adding in more of the rapid path generation features

testPathGeneration.py

@@ -0,0 +1,23 @@
+import matplotlib.pyplot as plt
+from utils.vehicles import *
+from utils.paths import *
+from utils.pathgeneration import *
+import scipy.io as sio
+
+
+veh = Vehicle(vehicleName = "shelley")
+
+#Create path object
+track = Path()
+track.loadFromMAT("maps/thunderhill_race.mat")
+
+#load in road bounds
+bounds = sio.loadmat('maps/thunderhill_bounds_shifted.mat')
+
+vp = RacingProfile(veh, track, 0.9)
+
+
+rpg = RapidPathGeneration(veh, track, bounds)
+path, vp = rpg.optimize()
+
+

utils.py

@@ -0,0 +1,110 @@
+import numpy as np
+import tiremodels as tm
+
+
+
+#### General utility functions useful across libraries ######
+
+
+#returns list of system matrices A, B
+def getAllSys(veh, Ux, K, ts):
+	A = []
+	B = []
+	N = len(Ux)
+
+	aF = np.zeros((N,1))
+	aR = np.zeros((N,1))
+
+    #Generate force lookup table
+    numTableValues = 250
+
+    #values where car is sliding
+    alphaFslide = np.abs(np.arctan(3*veh.muF*veh.m*veh.b/veh.L*veh.g/veh.Cf)) 
+    alphaRslide = np.abs(np.arctan(3*veh.muR*veh.m*veh.a/veh.L*veh.g/veh.Cr))
+
+    alphaFtable = np.linspace(-alphaFslide, alphaFslide, numTableValues)
+    alphaRtable = np.linspace(-alphaRslide, alphaRslide, numTableValues) # vector of rear alpha (rad)
+
+    FyFtable = tm.fiala(veh.Cf, veh.muF, veh.muF, alphaFtable, veh.FzF)
+    FyRtable = tm.fiala(veh.Cr, veh.muR, veh.muR, alphaRtable, veh.FzR)
+
+    #flip arrays so Fy is increasing - important for numpy interp!!
+    alphaFtable = np.flip(alphaFtable, 0)
+    alphaRtable = np.flip(alphaRtable, 0)
+    FyFtable = np.flip(FyFtable, 0) 
+    FyRtable = np.flip(FyRtable, 0)
+
+	for i in range(N):
+
+		FyFdes = veh.b / veh.L * veh.m * Ux[i]**2 * K[i]
+		FyRdes = veh.a / veh.L * veh.m * Ux[i]**2 * K[i]
+
+		aF[i] = _force2alpha(FyFtable, alphaFrontTable, FyFdes)
+		aR[i] = _force2alpha(FyRtable, alphaRearTable , FyRdes)
+
+		a, b, d = getAffineModel(Ux[i], K[i], ts[i], veh, aF[i], aR[i])
+
+		A.append[a]
+		B.append[b]
+		D.append[d]
+
+
+	return A, B, D
+
+
+def getAffineModel(Ux, K, ts, veh, aFhat, aRhat):
+	#unpack vehicle structure
+	a = veh.a
+	b = veh.b
+	m = veh.m
+	Cf = veh.Cf
+	Cr = veh.Cr
+	g = veh.g
+	L = a + b
+	Iz = veh.Iz
+	FzF = veh.FzF
+	FzR = veh.FzR
+	muF = veh.muF
+	muR = veh.muR
+
+
+	FyFhat = tm.fiala(Cf, muF, muF, afHat, FzF)
+	FyRhat = tm.fiala(Cr, muR, muR, arHat, FzR)
+	Cf = getLocalStiffness(afHat, Cf, muF, muF, FzF)
+	Cr = getLocalStiffness(arHat, Cr, muR, muR, FzR)
+
+	#States: e, deltaPsi, r, beta
+	Ac = np.array([ [0,  Ux,  0,  Ux] , [0, 0, 1, 0] , 
+		[0, 0, (-a**2*Cf - b**2*Cr)/(Ux*Iz), (b*Cr - a*Cf)/Iz] , 
+        [0,  0,(b*Cr - a*Cf)/(m*Ux**2)-1   , -(Cf+Cr)/(m*Ux)] ]) 
+
+
+	Bc = [[0], [0], [a*Cf/Iz], [Cf/(m*Ux)]]; 
+
+	##CONTINUE HERE ########
+
+	#dc = [[0], [-K*Ux]; (a*Cf*afHat - b*Cr*arHat)/Iz + (a*FyFhat-b*FyRhat)/Iz; (Cf*afHat + Cr*arHat)/(m*Ux) + (FyFhat + FyRhat)/(m*Ux)];
+
+	#[A B1] = myc2d(Ac, [Bc dc], ts);
+	#B = B1(:,1);
+	#d = B1(:,2);
+
+
+
+
+
+
+
+def _force2alpha(forceTable, alphaTable, Fdes):
+        if Fdes > max(forceTable):
+             Fdes = max(forceTable) - 1
+
+        elif Fdes < min(forceTable):
+             Fdes = min(forceTable) + 1
+
+        #note - need to slice to rank 0 for np to work
+        #note - x values must be increasing in numpy interp!!!
+        alpha = np.interp(Fdes, forceTable ,alphaTable)
+
+
+        return alpha

utils/pathgeneration.py

@@ -1,5 +1,6 @@
 import numpy as np 
 import cvxpy as cp
+import utils
 from velocityprofiles import *
 from simulation import *
 from control import *
@@ -102,13 +103,13 @@ def getCurvatureProfile(self):
 		lam2 = self.lam2
 
 		#get problem data
-		offset = np.norm( [self.path.posE[0] - self.path.posE[-1], self.path.posN[0] - self.path.posN[-1]] )
+		offset = np.linalg.norm( [self.path.posE[0] - self.path.posE[-1], self.path.posN[0] - self.path.posN[-1]] )
 		ds = np.diff(s)
 
-		_, ts = vp.getLapTime()
+		_, ts = getLapTime(self.vp.s, self.vp.Ux)
 
 		print('Generating Affine Tire Models ...')
-		A, B, d = self.getAllSys(veh, Ux, K, ts)
+		A, B, d = utils.getAllSys(veh, Ux, K, ts)
 
 		#Construct the problem here
 		print('Solving Convex Problem ...')

utils/velocityprofiles.py

@@ -162,6 +162,11 @@ def __init__(self, vehicle, path, friction = 0.3, vMax = 10, jerkLimit = 800, ds
 
 		self.getRacingProfile()
 
+	def resample(self, ds):
+		newS = np.arange(0, self.s[-1], ds)
+		self.Ux = np.interp(newS, self.s, self.Ux)
+		self.Ax = np.interp(newS, self.s, self.Ax)
+		self.s = newS
 
 	def getRacingProfile(self):
 		print("Generating Racing Profile")
@@ -761,6 +766,15 @@ def calcDerivatives(self, psi, theta, phi, dpsi_dsigma, dtheta_dsigma):
 			G[:,i] =     np.dot( M_BI ,  np.array([ [0] , [0] , [self.vehicle.g] ]) ).squeeze()
 
 		return Fv2, Fvdot, G
+
+
+############################# HELPER FUNCTIONS ##############################################################
+def getLapTime(s, Ux):
+
+	ts = np.concatenate(( s[1, np.newaxis], np.diff(s) )) / Ux
+	lapTime = np.sum(ts)
+
+	return lapTime, ts