def solve(var):
# initialize model
reaction_model = GEKKO()
# set model time as time gathered data
reaction_model.time = time
# Constants
R = reaction_model.Const(8.3145) # Gas constant J / mol K
# Parameters
T = reaction_model.Param(temperature)
# Fixed Variables to change
# bounds
E_a_1 = reaction_model.FV(E_a_1_initial_guess)
E_a_2 = reaction_model.FV(E_a_2_initial_guess)
A_1 = reaction_model.FV(A_1_initial_guess)
A_2 = reaction_model.FV(A_2_initial_guess)
alpha = reaction_model.FV(alpha_initial_guess)
beta = reaction_model.FV(beta_initial_guess)
# NO bounds
# state Variables
Ph_3_minus = reaction_model.SV(Ph_3_minus_initial)
# variable we will use to regress other Parameters
Ph_2_minus = reaction_model.SV(Ph_2_minus_initial)
# intermediates
k1 = reaction_model.Intermediate(A_1 * reaction_model.exp(-E_a_1 /
(R * T)))
k2 = reaction_model.Intermediate(A_2 * reaction_model.exp(-E_a_2 /
(R * T)))
r1 = reaction_model.Intermediate(k1 * Ph_2_minus**alpha)
r2 = reaction_model.Intermediate(k2 * Ph_3_minus**beta)
# equations
reaction_model.Equations(
[Ph_2_minus.dt() == r2 - r1,
Ph_3_minus.dt() == r1 - r2])
# parameter options
# controlled variable options
# model options and other to solve
reaction_model.options.IMODE = 4 # set up dynamic simulation
reaction_model.options.NODES = 2 # number of nodes for collocation equations
reaction_model.options.SOLVER = 1 # use APOPT active set non-linear solverreaction_model.options.EV_TYPE = 2 # use l-1 norm rather than 2 norm
reaction_model.solve(disp=True)
return Ph_2_minus.value
def solve11(): # error
# Moving Horizon Estimation
# Estimator Model
m = GEKKO()
m.time = p.time
# Parameters
m.u = m.MV(value=u_meas) #input
m.K = m.FV(value=1, lb=1, ub=3) # gain
m.tau = m.FV(value=5, lb=1, ub=10) # time constant
# Variables
m.x = m.SV() #state variable
m.y = m.CV(value=y_meas) #measurement
# Equations
m.Equations([m.tau * m.x.dt() == -m.x + m.u, m.y == m.K * m.x])
# Options
m.options.IMODE = 5 #MHE
m.options.EV_TYPE = 1
# STATUS = 0, optimizer doesn't adjust value
# STATUS = 1, optimizer can adjust
m.u.STATUS = 0
m.K.STATUS = 1
m.tau.STATUS = 1
m.y.STATUS = 1
# FSTATUS = 0, no measurement
# FSTATUS = 1, measurement used to update model
m.u.FSTATUS = 1
m.K.FSTATUS = 0
m.tau.FSTATUS = 0
m.y.FSTATUS = 1
# DMAX = maximum movement each cycle
m.K.DMAX = 2.0
m.tau.DMAX = 4.0
# MEAS_GAP = dead-band for measurement / model mismatch
m.y.MEAS_GAP = 0.25
# solve
m.solve(disp=False)
# Plot results
plt.subplot(2, 1, 1)
plt.plot(m.time, u_meas, 'b:', label='Input (u) meas')
plt.legend()
plt.subplot(2, 1, 2)
plt.plot(m.time, y_meas, 'gx', label='Output (y) meas')
plt.plot(p.time, p.y.value, 'k-', label='Output (y) actual')
plt.plot(m.time, m.y.value, 'r--', label='Output (y) estimated')
plt.legend()
plt.show()
def solve08():
# Nonlinear Regression
# measurements
xm = np.array([0, 1, 2, 3, 4, 5])
ym = np.array([0.1, 0.2, 0.3, 0.5, 0.8, 2.0])
# GEKKO model
m = GEKKO()
# parameters
x = m.Param(value=xm)
a = m.FV()
a.STATUS = 1
# variables
y = m.CV(value=ym)
y.FSTATUS = 1
# regression equation
m.Equation(y == 0.1 * m.exp(a * x))
# regression mode
m.options.IMODE = 2
# optimize
m.solve(disp=False)
# print parameters
print('Optimized, a = ' + str(a.value[0]))
plt.plot(xm, ym, 'bo')
plt.plot(xm, y.value, 'r-')
plt.show()
def solve07():
# Linear and Polynomial Regression
xm = np.array([0, 1, 2, 3, 4, 5])
ym = np.array([0.1, 0.2, 0.3, 0.5, 1.0, 0.9])
xm = np.array([0, 1, 2, 3, 4, 5])
ym = np.array([0.1, 0.2, 0.3, 0.5, 0.8, 2.0])
#### Solution
m = GEKKO()
m.options.IMODE = 2
# coefficients
c = [m.FV(value=0) for i in range(4)]
x = m.Param(value=xm)
y = m.CV(value=ym)
y.FSTATUS = 1
# polynomial model
m.Equation(y == c[0] + c[1] * x + c[2] * x**2 + c[3] * x**3)
# linear regression
c[0].STATUS = 1
c[1].STATUS = 1
m.solve(disp=False)
p1 = [c[1].value[0], c[0].value[0]]
# quadratic
c[2].STATUS = 1
m.solve(disp=False)
p2 = [c[2].value[0], c[1].value[0], c[0].value[0]]
# cubic
c[3].STATUS = 1
m.solve(disp=False)
p3 = [c[3].value[0], c[2].value[0], c[1].value[0], c[0].value[0]]
# plot fit
plt.plot(xm, ym, 'ko', markersize=10)
xp = np.linspace(0, 5, 100)
plt.plot(xp, np.polyval(p1, xp), 'b--', linewidth=2)
plt.plot(xp, np.polyval(p2, xp), 'r--', linewidth=3)
plt.plot(xp, np.polyval(p3, xp), 'g:', linewidth=2)
plt.legend(['Data', 'Linear', 'Quadratic', 'Cubic'], loc='best')
plt.xlabel('x')
plt.ylabel('y')
plt.show()
def jennings():
m = GEKKO()
nt = 501
m.time = np.linspace(0,1,nt)
x1 = m.Var(value=3.1415/2)
x2 = m.Var(value=4)
x3 = m.Var(value=0)
u = m.MV(lb=-2,ub=2)
u.STATUS = 1
scale = m.FV(value=1,lb=0.1,ub=100)
scale.status = 1
m.Equation(x1.dt()/scale == u)
m.Equation(x2.dt()/scale == m.cos(x1))
m.Equation(x3.dt()/scale == m.sin(x1))
#z = m.FV(value=0)
#z.STATUS = 0
#m.Connection(z,x2,pos2='end')
#m.Connection(z,x3,pos2='end')
m.fix(x2,nt-1,0)
m.fix(x3,nt-1,0)
m.Obj(scale)
m.options.ICD_CALC = 1
m.options.IMODE = 6
m.solve(disp=False)
assert test.like(x1.value, [1.57075, 1.584488, 1.597806, 1.611169, 1.624564, 1.63798, 1.651413, 1.664859, 1.678313, 1.69177, 1.705228, 1.718683, 1.732134, 1.74558, 1.759019, 1.77245, 1.785874, 1.799288, 1.812693, 1.826088, 1.839472, 1.852847, 1.866211, 1.879564, 1.892906, 1.906237, 1.919557, 1.932866, 1.946163, 1.959449,1.972723, 1.985986, 1.999236, 2.012475, 2.025701, 2.038915, 2.052117, 2.065306, 2.078482, 2.091646, 2.104796, 2.117933, 2.131056, 2.144166, 2.157263, 2.170345, 2.183413, 2.196467, 2.209506, 2.222531, 2.235541,2.248536, 2.261516, 2.274481, 2.287431, 2.300365, 2.313284, 2.326187, 2.339074, 2.351945, 2.3648, 2.377639, 2.390461, 2.403268, 2.416057, 2.428831, 2.441588, 2.454328, 2.467051, 2.479758, 2.492448, 2.505121, 2.517777, 2.530416, 2.543038, 2.555644, 2.568232, 2.580803, 2.593358, 2.605895, 2.618415, 2.630919, 2.643405, 2.655875, 2.668327, 2.680763, 2.693182, 2.705585, 2.71797, 2.73034, 2.742692, 2.755028, 2.767348, 2.779651, 2.791939, 2.80421, 2.816465, 2.828704, 2.840928, 2.853136, 2.865328, 2.877505, 2.889667, 2.901814,2.913946, 2.926063, 2.938165, 2.950253, 2.962327, 2.974387, 2.986432, 2.998464, 3.010482, 3.022487, 3.034478, 3.046457, 3.058422, 3.070375, 3.082316, 3.094244, 3.10616, 3.118064, 3.129956, 3.141837, 3.153707, 3.165566, 3.177413, 3.18925, 3.201077, 3.212893, 3.2247, 3.236496, 3.248283, 3.26006, 3.271829, 3.283588,3.295338, 3.30708, 3.318814, 3.330539, 3.342256, 3.353966, 3.365668, 3.377363, 3.38905, 3.40073, 3.412404, 3.424071, 3.435732, 3.447386, 3.459034, 3.470677, 3.482313, 3.493944, 3.50557, 3.51719, 3.528805, 3.540416, 3.552021, 3.563622, 3.575219, 3.586811, 3.598399, 3.609982, 3.621562, 3.633138, 3.64471, 3.656278, 3.667842, 3.679404, 3.690961, 3.702516, 3.714067, 3.725614, 3.737159, 3.7487, 3.760239, 3.771774, 3.783306, 3.794835, 3.806361, 3.817884, 3.829404, 3.840921, 3.852434, 3.863944, 3.875451, 3.886955, 3.898455, 3.909952, 3.921445, 3.932934, 3.944419, 3.9559, 3.967377, 3.97885, 3.990318, 4.00178, 4.013238, 4.024691, 4.036137, 4.047578, 4.059013, 4.070441, 4.081862, 4.093275, 4.104681, 4.116078, 4.127467, 4.138846, 4.150215, 4.161573, 4.172921, 4.184256, 4.195578, 4.206887, 4.218181, 4.229459, 4.24072, 4.251962, 4.263186, 4.274389, 4.28557, 4.296727, 4.30786, 4.318965, 4.330041, 4.341085, 4.352094, 4.363065, 4.373994, 4.384877,4.395709, 4.406487, 4.417206, 4.427862, 4.438456, 4.448986, 4.459455, 4.469864, 4.480206, 4.490454, 4.500528, 4.510246, 4.519226, 4.526707, 4.531218, 4.529966, 4.518415, 4.494345, 4.47313, 4.484098, 4.508164, 4.521127, 4.522516, 4.517303, 4.508851, 4.49891, 4.488353, 4.477589, 4.466791, 4.456028, 4.445307, 4.434603, 4.423888, 4.413144, 4.40236, 4.39153, 4.380652, 4.369725, 4.358754, 4.347739, 4.336685, 4.325595, 4.314471, 4.303316, 4.292133, 4.280923, 4.269689, 4.258433, 4.247155, 4.235857, 4.22454, 4.213206, 4.201856,4.190491, 4.179112, 4.167719, 4.156314, 4.144898, 4.133471, 4.122034, 4.110588, 4.099132, 4.087668, 4.076196, 4.064716, 4.05323, 4.041736, 4.030236, 4.01873, 4.007219, 3.995702, 3.984179, 3.972652, 3.96112, 3.949583, 3.938042, 3.926497, 3.914947, 3.903394, 3.891837, 3.880277, 3.868713, 3.857146, 3.845575, 3.834001, 3.822424, 3.810844, 3.79926, 3.787674, 3.776085, 3.764493, 3.752897, 3.741299, 3.729698, 3.718093, 3.706486, 3.694876, 3.683262, 3.671645, 3.660026, 3.648402, 3.636776, 3.625146, 3.613512, 3.601875, 3.590235, 3.57859, 3.566942, 3.555289, 3.543633, 3.531972, 3.520307, 3.508637, 3.496963, 3.485284, 3.473601, 3.461912, 3.450218, 3.438519, 3.426814, 3.415104, 3.403388, 3.391666, 3.379938, 3.368204, 3.356463, 3.344716,3.332962, 3.321201, 3.309433, 3.297658, 3.285875, 3.274085, 3.262286, 3.25048, 3.238666, 3.226843, 3.215012, 3.203172, 3.191323, 3.179465, 3.167597, 3.155721, 3.143835, 3.131938, 3.120032, 3.108116, 3.09619, 3.084253, 3.072305, 3.060347, 3.048378, 3.036398, 3.024406, 3.012403, 3.000388, 2.988362, 2.976324, 2.964274, 2.952211, 2.940137, 2.92805, 2.915951, 2.903839, 2.891714, 2.879576, 2.867426, 2.855262, 2.843086, 2.830896, 2.818692, 2.806476, 2.794246, 2.782002, 2.769745, 2.757474, 2.745189, 2.732891, 2.720579, 2.708253, 2.695913, 2.68356, 2.671192, 2.658811, 2.646416, 2.634007, 2.621585, 2.609148, 2.596698, 2.584234, 2.571757, 2.559266, 2.546761, 2.534243, 2.521711, 2.509166, 2.496608, 2.484036, 2.471452, 2.458854, 2.446244, 2.433621, 2.420985, 2.408337, 2.395676, 2.383004, 2.370319, 2.357622, 2.344913, 2.332193, 2.319461, 2.306718, 2.293964, 2.2812, 2.268424, 2.255638, 2.242842, 2.230035, 2.217219, 2.204393, 2.191558, 2.178714, 2.165861, 2.152999, 2.140129, 2.12725, 2.114364, 2.101471, 2.08857, 2.075662, 2.062747, 2.049826, 2.036899, 2.023966, 2.011028, 1.998085, 1.985137, 1.972185, 1.959228, 1.946268, 1.933305, 1.920338, 1.907369, 1.894398, 1.881425, 1.868451, 1.855476, 1.8425, 1.829524, 1.816549, 1.803574, 1.7906, 1.777628, 1.764659, 1.751692, 1.738728, 1.725768, 1.712812, 1.699862, 1.686917, 1.673978, 1.661046, 1.648122, 1.635207, 1.622301, 1.609406, 1.596522, 1.583652, 1.570796])
assert test.like(x2.value, [4.0, 3.999835, 3.99951, 3.999024, 3.998377, 3.997569, 3.9966, 3.995469, 3.994178, 3.992725, 3.991112, 3.989338, 3.987405, 3.985311, 3.983059, 3.980648, 3.978079, 3.975353, 3.972469, 3.96943, 3.966234, 3.962884, 3.95938, 3.955722, 3.951911, 3.947949, 3.943835, 3.939572, 3.935159, 3.930598, 3.925889, 3.921033, 3.916033, 3.910887, 3.905599, 3.900167, 3.894595, 3.888882, 3.88303, 3.87704, 3.870914, 3.864652, 3.858255, 3.851725, 3.845064, 3.838272, 3.83135, 3.824301, 3.817125, 3.809824, 3.802398, 3.794851, 3.787182, 3.779394, 3.771487, 3.763464, 3.755325, 3.747073, 3.738709, 3.730233, 3.721649, 3.712957, 3.704159, 3.695256, 3.686251, 3.677144, 3.667938, 3.658633, 3.649233, 3.639737, 3.630148, 3.620468, 3.610698, 3.60084, 3.590896, 3.580867, 3.570755, 3.560561, 3.550288, 3.539938, 3.529511, 3.519009, 3.508435, 3.497791, 3.487077, 3.476295, 3.465448, 3.454537, 3.443564, 3.432531, 3.421439, 3.41029, 3.399086, 3.387829, 3.37652, 3.365162,3.353756, 3.342303, 3.330806, 3.319266, 3.307686, 3.296066, 3.284409, 3.272716, 3.26099, 3.249231, 3.237442, 3.225625, 3.213781, 3.201912, 3.190019, 3.178105, 3.166172, 3.15422, 3.142252, 3.130269, 3.118274, 3.106267, 3.094251, 3.082228, 3.070198, 3.058165, 3.046128, 3.034091, 3.022055, 3.010022, 2.997992, 2.985969, 2.973953, 2.961947, 2.949951, 2.937968, 2.926, 2.914047, 2.902112, 2.890196, 2.878301, 2.866429, 2.85458, 2.842757, 2.830962, 2.819195, 2.807459, 2.795755, 2.784085, 2.77245, 2.760851, 2.749291, 2.737771, 2.726293, 2.714857, 2.703466, 2.692121, 2.680823, 2.669575, 2.658377, 2.647231, 2.636139, 2.625101, 2.61412, 2.603197, 2.592334, 2.581531, 2.57079, 2.560113, 2.549502, 2.538956, 2.528479, 2.51807, 2.507732, 2.497467, 2.487274, 2.477156, 2.467114, 2.45715, 2.447264, 2.437457, 2.427732, 2.41809, 2.408531, 2.399057, 2.38967, 2.380369, 2.371158, 2.362036, 2.353005, 2.344066, 2.335221, 2.32647, 2.317815, 2.309256, 2.300796, 2.292434, 2.284172, 2.276011, 2.267952, 2.259997, 2.252145, 2.244398, 2.236758, 2.229224, 2.221798, 2.214481, 2.207274, 2.200177, 2.193192, 2.186319, 2.179559, 2.172913, 2.166382, 2.159966, 2.153666, 2.147482, 2.141417, 2.135469, 2.12964, 2.123931, 2.118341, 2.112872, 2.107523, 2.102296, 2.097191, 2.092208, 2.087347, 2.08261, 2.077995, 2.073504, 2.069137, 2.064893, 2.060773, 2.056777, 2.052905, 2.049157, 2.045532, 2.04203, 2.038651, 2.035395, 2.032261, 2.029249, 2.026358, 2.023588, 2.020939, 2.018407, 2.015991, 2.01368, 2.011458, 2.009289, 2.007105, 2.004785, 2.002181, 1.999329, 1.996604, 1.994163, 1.991875, 1.989603, 1.98727, 1.984837, 1.982287, 1.979612, 1.976812, 1.973885, 1.970833, 1.967656, 1.964356, 1.960931, 1.957382, 1.95371, 1.949914, 1.945993, 1.941949, 1.93778, 1.933488, 1.929071, 1.92453, 1.919866, 1.915078, 1.910167, 1.905133, 1.899977, 1.894698, 1.889298, 1.883777, 1.878135, 1.872372, 1.866491, 1.86049, 1.854371, 1.848134, 1.84178, 1.83531, 1.828724, 1.822024, 1.815209, 1.808282, 1.801242, 1.79409, 1.786828, 1.779455, 1.771974, 1.764386, 1.75669, 1.748888, 1.740981, 1.73297, 1.724855, 1.716639, 1.708322, 1.699906, 1.69139, 1.682776, 1.674067, 1.665261, 1.656362, 1.647369, 1.638284, 1.629109, 1.619844, 1.61049, 1.60105, 1.591523, 1.581912, 1.572218, 1.562442, 1.552585, 1.542648, 1.532633, 1.522542, 1.512375, 1.502134, 1.49182, 1.481434, 1.470979, 1.460455, 1.449864, 1.439207, 1.428486, 1.417701, 1.406856, 1.39595, 1.384985, 1.373964, 1.362886, 1.351755, 1.340571, 1.329336, 1.318051, 1.306718, 1.295338, 1.283913, 1.272445, 1.260935, 1.249384, 1.237794, 1.226167, 1.214505, 1.202808, 1.191079, 1.179318, 1.167529, 1.155712, 1.143868, 1.132, 1.12011, 1.108198, 1.096266, 1.084317, 1.072351, 1.060371, 1.048377, 1.036373, 1.024358, 1.012336, 1.000308, 0.9882749, 0.9762391, 0.9642021, 0.9521656, 0.9401314, 0.9281011, 0.9160765, 0.9040592, 0.8920511, 0.8800538, 0.868069, 0.8560985, 0.844144, 0.8322073, 0.8202901, 0.8083941, 0.7965211, 0.7846728, 0.772851, 0.7610574, 0.7492938, 0.7375619, 0.7258634, 0.7142003, 0.7025741, 0.6909866, 0.6794396, 0.6679349, 0.6564742, 0.6450593, 0.6336918, 0.6223737, 0.6111065, 0.5998921, 0.5887323, 0.5776286, 0.566583, 0.5555971, 0.5446727, 0.5338115, 0.5230152, 0.5122855, 0.5016242, 0.491033, 0.4805136, 0.4700677, 0.459697, 0.4494031,0.4391878, 0.4290528, 0.4189996, 0.40903, 0.3991457, 0.3893482, 0.3796392, 0.3700203, 0.3604931, 0.3510593, 0.3417204, 0.332478, 0.3233336, 0.314289, 0.3053455, 0.2965047, 0.2877681, 0.2791373, 0.2706137, 0.2621988, 0.253894, 0.2457009, 0.2376208, 0.2296551, 0.2218053, 0.2140727, 0.2064586, 0.1989645, 0.1915917, 0.1843413, 0.1772149, 0.1702135, 0.1633384, 0.1565909, 0.1499722, 0.1434834, 0.1371257, 0.1309002, 0.1248081, 0.1188504, 0.1130282, 0.1073426, 0.1017945, 0.09638493, 0.0911149, 0.08598533, 0.08099713, 0.07615118, 0.07144836, 0.06688948, 0.06247536, 0.05820678, 0.05408449, 0.05010921, 0.04628163, 0.04260242, 0.03907221, 0.03569162, 0.0324612, 0.02938152, 0.02645307, 0.02367636, 0.02105183, 0.0185799, 0.01626096, 0.01409538, 0.01208347, 0.01022553, 0.008521817, 0.006972563, 0.005577959, 0.004338162, 0.003253297, 0.002323452, 0.001548677, 0.000928989, 0.0004643624, 0.0001547344, 3.075445e-17, 0.0])
assert test.like(x3.value, [0.0, 0.0120359, 0.02406854, 0.03609576, 0.04811539, 0.06012527, 0.0721232, 0.08410702, 0.09607454, 0.1080236, 0.119952, 0.1318577, 0.1437384, 0.155592, 0.1674164, 0.1792096, 0.1909693, 0.2026934, 0.21438, 0.2260269, 0.2376321, 0.2491935, 0.2607091, 0.2721769, 0.2835949, 0.294961, 0.3062734, 0.31753, 0.3287289, 0.3398682, 0.350946, 0.3619604, 0.3729094, 0.3837913, 0.3946042, 0.4053463, 0.4160157, 0.4266107, 0.4371296, 0.4475704, 0.4579317, 0.4682115, 0.4784083, 0.4885203, 0.498546, 0.5084836, 0.5183317, 0.5280885, 0.5377527, 0.5473225, 0.5567965, 0.5661733, 0.5754513, 0.5846291, 0.5937052, 0.6026784, 0.6115471, 0.6203101,0.6289661, 0.6375136, 0.6459515, 0.6542785, 0.6624934, 0.6705949, 0.6785819, 0.6864532, 0.6942078, 0.7018444, 0.7093619, 0.7167595, 0.7240359, 0.7311903, 0.7382216, 0.7451288, 0.751911, 0.7585674, 0.765097, 0.7714989, 0.7777724, 0.7839166, 0.7899307, 0.795814, 0.8015657, 0.8071851, 0.8126715, 0.8180243, 0.8232428, 0.8283263, 0.8332743, 0.8380862, 0.8427614, 0.8472995, 0.8516999, 0.8559621, 0.8600856, 0.8640701, 0.8679151, 0.8716202, 0.875185, 0.8786092, 0.8818925, 0.8850345, 0.888035, 0.8908936, 0.8936102, 0.8961845, 0.8986163, 0.9009054, 0.9030517, 0.905055, 0.9069152, 0.9086321, 0.9102058, 0.9116361, 0.912923, 0.9140664, 0.9150664, 0.9159229, 0.916636, 0.9172057, 0.9176321, 0.9179153, 0.9180554, 0.9180524, 0.9179066, 0.9176181, 0.917187, 0.9166136, 0.915898, 0.9150404, 0.9140412, 0.9129006, 0.9116188, 0.9101961, 0.9086329, 0.9069294, 0.9050861, 0.9031032, 0.9009811, 0.8987203, 0.896321, 0.8937839, 0.8911092, 0.8882974, 0.8853491, 0.8822646, 0.8790446, 0.8756894, 0.8721997, 0.8685759, 0.8648187, 0.8609286, 0.8569063, 0.8527522, 0.8484671, 0.8440516, 0.8395063, 0.8348319, 0.8300291, 0.8250986, 0.8200411, 0.8148573, 0.8095479, 0.8041138,0.7985557, 0.7928743, 0.7870706, 0.7811452, 0.7750991, 0.7689331, 0.7626479, 0.7562446, 0.749724, 0.743087, 0.7363345, 0.7294674, 0.7224868, 0.7153934, 0.7081884, 0.7008728, 0.6934474, 0.6859134, 0.6782717, 0.6705234, 0.6626696, 0.6547113, 0.6466496, 0.6384856, 0.6302205, 0.6218553, 0.6133912, 0.6048293, 0.5961707, 0.5874168, 0.5785686, 0.5696274, 0.5605944, 0.5514707, 0.5422577, 0.5329566, 0.5235686, 0.5140951, 0.5045372, 0.4948964, 0.4851739, 0.475371, 0.4654891, 0.4555295, 0.4454936, 0.4353827, 0.4251982, 0.4149415,0.4046139, 0.3942169, 0.3837519, 0.3732204, 0.3626236, 0.3519632, 0.3412405, 0.3304569, 0.3196141, 0.3087133, 0.2977562, 0.2867441, 0.2756786, 0.2645612, 0.2533933, 0.2421766, 0.2309124, 0.2196023, 0.2082479, 0.1968507, 0.1854123, 0.173934, 0.1624176, 0.1508646, 0.1392763, 0.1276545, 0.1160004, 0.1043157, 0.09260163, 0.08085983, 0.06909194, 0.0573, 0.04548684, 0.03365672, 0.02181669, 0.009979397, -0.001831889, -0.01358391, -0.02527805, -0.03700278, -0.04878966, -0.06060719, -0.07242789, -0.08423659, -0.09602515, -0.1077889, -0.1195251, -0.1312319, -0.1429077, -0.1545514, -0.1661616, -0.1777372, -0.1892768, -0.2007789, -0.212242, -0.2236647, -0.2350455, -0.2463827, -0.2576749, -0.2689205, -0.2801179, -0.2912657, -0.3023623, -0.3134061, -0.3243957, -0.3353296, -0.3462063, -0.3570242, -0.3677819, -0.3784778, -0.3891107, -0.3996789, -0.410181, -0.4206156, -0.4309812, -0.4412765, -0.4514999, -0.4616502, -0.4717258, -0.4817255, -0.4916479, -0.5014915, -0.511255, -0.5209372, -0.5305366, -0.5400519, -0.5494819, -0.5588253, -0.5680807, -0.5772469, -0.5863227, -0.5953067, -0.6041978, -0.6129947, -0.6216963, -0.6303013, -0.6388086, -0.6472169, -0.6555252, -0.6637322, -0.671837, -0.6798382, -0.6877349, -0.695526, -0.7032104, -0.7107869, -0.7182547, -0.7256126, -0.7328596, -0.7399948, -0.7470171, -0.7539256, -0.7607193, -0.7673973, -0.7739586, -0.7804023,-0.7867276, -0.7929335, -0.7990192, -0.8049837, -0.8108264, -0.8165463, -0.8221427, -0.8276147, -0.8329615, -0.8381825, -0.8432768, -0.8482438, -0.8530826, -0.8577926, -0.8623732, -0.8668236, -0.8711432, -0.8753313, -0.8793874, -0.8833108, -0.8871009, -0.8907571, -0.8942789, -0.8976658, -0.9009172, -0.9040325, -0.9070114, -0.9098533, -0.9125577, -0.9151243, -0.9175525, -0.919842, -0.9219924, -0.9240032, -0.9258741, -0.9276048, -0.929195, -0.9306443, -0.9319524, -0.933119, -0.9341439, -0.9350269, -0.9357676, -0.936366, -0.9368217, -0.9371347, -0.9373048, -0.9373318, -0.9372156, -0.9369561, -0.9365532, -0.9360069, -0.935317, -0.9344837, -0.9335068, -0.9323864, -0.9311225, -0.9297151, -0.9281644, -0.9264704, -0.9246331, -0.9226528, -0.9205296, -0.9182636, -0.9158551, -0.9133041, -0.9106111, -0.9077761, -0.9047995, -0.9016816, -0.8984226, -0.8950229, -0.8914829, -0.8878029, -0.8839834, -0.8800246, -0.8759272, -0.8716915, -0.8673179, -0.8628072, -0.8581596, -0.8533759, -0.8484565, -0.8434021, -0.8382133, -0.8328907, -0.827435, -0.8218469,-0.8161271, -0.8102762, -0.8042952, -0.7981847, -0.7919456, -0.7855787, -0.7790847, -0.7724648, -0.7657196, -0.7588501, -0.7518573, -0.7447422, -0.7375057, -0.7301489, -0.7226727, -0.7150783, -0.7073667, -0.6995391, -0.6915966, -0.6835403, -0.6753715, -0.6670912, -0.6587008, -0.6502015, -0.6415946, -0.6328813, -0.624063, -0.615141, -0.6061167, -0.5969914, -0.5877666, -0.5784437, -0.5690241, -0.5595093, -0.5499009, -0.5402002, -0.5304088, -0.5205284, -0.5105604, -0.5005064, -0.4903681, -0.480147, -0.4698449, -0.4594634,-0.4490041, -0.4384688, -0.4278592, -0.4171771, -0.4064241, -0.395602, -0.3847127, -0.3737579, -0.3627394, -0.3516591, -0.3405188, -0.3293203, -0.3180655, -0.3067564, -0.2953947, -0.2839825, -0.2725215, -0.2610138, -0.2494612, -0.2378658, -0.2262294, -0.214554, -0.2028417, -0.1910942, -0.1793138, -0.1675022, -0.1556616, -0.1437939, -0.1319011, -0.1199853, -0.1080484, -0.09609239, -0.08411938, -0.07213133, -0.06013027, -0.04811821, -0.03609714, -0.02406908, -0.01203603, -4.752194e-19, 0.0])
assert test.like(u.value, [0.0, 1.141285, 1.10648, 1.110143, 1.112781, 1.114577, 1.115979, 1.11707, 1.117704, 1.117975, 1.117991, 1.117814, 1.117485, 1.117034, 1.116483, 1.115854, 1.11516, 1.114415, 1.11363, 1.112812, 1.11197, 1.111107, 1.110227, 1.109333, 1.108427, 1.107511, 1.106585, 1.105649, 1.104703, 1.103748, 1.102782, 1.101806, 1.10082, 1.099822, 1.098812, 1.09779, 1.096755, 1.095707, 1.094646, 1.093571, 1.092483, 1.09138, 1.090264, 1.089134, 1.087989, 1.086831, 1.085659, 1.084474, 1.083275, 1.082063, 1.080838, 1.0796, 1.07835, 1.077089,1.075816, 1.074531, 1.073236, 1.071931, 1.070616, 1.069292, 1.067958, 1.066616, 1.065266, 1.063909, 1.062544, 1.061173, 1.059796, 1.058413, 1.057025, 1.055633, 1.054236, 1.052836, 1.051433, 1.050027, 1.048619,1.047209, 1.045799, 1.044387, 1.042976, 1.041565, 1.040155, 1.038746, 1.037339, 1.035935, 1.034533, 1.033135, 1.031741, 1.030351, 1.028966, 1.027586, 1.026213, 1.024846, 1.023485, 1.022133, 1.020788, 1.019452,1.018124, 1.016806, 1.015498, 1.0142, 1.012913, 1.011636, 1.010372, 1.009119, 1.007878, 1.00665, 1.005435, 1.004233, 1.003045, 1.00187, 1.00071, 0.9995647, 0.9984339, 0.9973181, 0.9962175, 0.9951325, 0.9940632, 0.9930098, 0.9919725, 0.9909515, 0.989947, 0.988959, 0.9879879, 0.9870336, 0.9860962, 0.9851759, 0.9842728, 0.9833869, 0.9825183, 0.9816669, 0.9808329, 0.9800163, 0.9792169, 0.9784349, 0.9776702, 0.9769227, 0.9761924, 0.9754792, 0.974783, 0.9741037, 0.9734412, 0.9727952, 0.9721658, 0.9715527, 0.9709556, 0.9703745, 0.9698091, 0.9692592, 0.9687246, 0.968205, 0.9677001, 0.9672097, 0.9667334, 0.9662711, 0.9658223, 0.9653868, 0.9649641, 0.9645541, 0.9641561, 0.96377, 0.9633953, 0.9630316, 0.9626785, 0.9623354, 0.9620021, 0.9616779, 0.9613625, 0.9610553, 0.9607557, 0.9604633, 0.9601775, 0.9598977, 0.9596233, 0.9593537, 0.9590883, 0.9588264, 0.9585674, 0.9583105, 0.958055, 0.9578002, 0.9575453, 0.9572895, 0.957032, 0.9567719, 0.9565084, 0.9562405, 0.9559673, 0.9556878, 0.9554011, 0.955106, 0.9548014, 0.9544864, 0.9541596, 0.9538198, 0.9534658, 0.9530962, 0.9527097, 0.9523047, 0.9518798, 0.9514333, 0.9509635, 0.9504686, 0.9499467, 0.9493957, 0.9488135, 0.9481976, 0.9475455, 0.9468544, 0.9461213, 0.9453429, 0.9445155, 0.943635, 0.942697, 0.9416964, 0.9406274, 0.9394835, 0.9382575, 0.9369408, 0.9355238, 0.9340117, 0.9324096, 0.9307068, 0.9288909, 0.926947, 0.9248581, 0.9226039, 0.9201612, 0.9175035, 0.9146009, 0.9114214, 0.9079325, 0.9041047, 0.8999173, 0.8953683, 0.890486, 0.8853435, 0.8800661, 0.8748178, 0.8697326, 0.8647418, 0.8592182, 0.8513293, 0.8368962, 0.8073544, 0.7460418, 0.6214726, 0.3747623, -0.1040045, -0.959582, -1.99966, -1.762497, 0.9112015, 1.999294, 1.076977, 0.1153686, -0.4330951, -0.7021474, -0.8258591, -0.8770839, -0.8942374, -0.8970691, -0.8940893, -0.8906796, -0.8892951, -0.8901524, -0.8925633, -0.8959036, -0.8997412, -0.9037485, -0.9077083, -0.9114936, -0.9150413, -0.9183291, -0.9213588, -0.9241454, -0.9267093, -0.9290731, -0.9312586, -0.9332859, -0.9351732, -0.9369363, -0.9385891, -0.9401436, -0.941596, -0.942943, -0.9441951, -0.9453615, -0.9464503, -0.9474686, -0.9484224, -0.9493172, -0.950158, -0.9509489, -0.951694, -0.9523965, -0.9530598, -0.9536866, -0.9542795, -0.954841, -0.9553732, -0.9558782, -0.9563579, -0.9568141, -0.9572484, -0.9576625, -0.9580577, -0.9584355, -0.9587973, -0.9591442, -0.9594776, -0.9597987, -0.9601084, -0.9604079, -0.9606983, -0.9609805, -0.9612555, -0.9615242, -0.9617875, -0.9620463, -0.9623015, -0.9625537, -0.9628039, -0.9630528, -0.9633011, -0.9635496, -0.9637989, -0.9640496, -0.9643025, -0.9645582, -0.9648173, -0.9650803, -0.9653479, -0.9656206, -0.9658989, -0.9661833, -0.9664744, -0.9667726, -0.9670785, -0.9673923, -0.9677147, -0.9680459, -0.9683865, -0.9687368, -0.9690971, -0.9694679, -0.9698495, -0.9702422, -0.9706464, -0.9710623, -0.9714903, -0.9719306, -0.9723834, -0.9728492, -0.973328, -0.9738201, -0.9743258, -0.9748451, -0.9753784, -0.9759258, -0.9764873, -0.9770633, -0.9776538, -0.9782589, -0.9788787, -0.9795133, -0.9801628, -0.9808271, -0.9815065, -0.9822007, -0.9829099, -0.9836341, -0.9843731, -0.9851269, -0.9858955, -0.9866788, -0.9874767, -0.9882891, -0.9891158, -0.9899567, -0.9908117, -0.9916807, -0.9925633, -0.9934595, -0.994369,-0.9952916, -0.9962271, -0.9971753, -0.9981358, -0.9991084, -1.000093, -1.001089, -1.002096, -1.003114, -1.004142, -1.005181, -1.006229, -1.007287, -1.008354, -1.00943, -1.010514, -1.011605, -1.012704, -1.01381, -1.014923, -1.016041, -1.017165, -1.018294, -1.019427, -1.020564, -1.021705, -1.022849, -1.023995, -1.025143, -1.026292, -1.027442, -1.028592, -1.029741, -1.030889, -1.032036, -1.033181, -1.034323, -1.035462, -1.036598, -1.037729, -1.038855, -1.039976, -1.041091, -1.042199, -1.043301, -1.044396, -1.045482, -1.04656, -1.047629, -1.048688, -1.049737, -1.050776, -1.051804, -1.05282, -1.053824, -1.054815, -1.055793, -1.056757, -1.057707, -1.058642, -1.059562, -1.060466, -1.061354, -1.062225, -1.063079, -1.063914, -1.064731, -1.06553, -1.066308, -1.067067, -1.067805, -1.068522, -1.069217, -1.06989, -1.070541, -1.071168, -1.071771, -1.07235, -1.072905, -1.073434, -1.073937, -1.074413, -1.074863, -1.075285, -1.075679, -1.076044, -1.07638, -1.076686, -1.076962, -1.077206, -1.07742, -1.077601, -1.077749, -1.077864, -1.077944, -1.07799, -1.078, -1.077973, -1.077909, -1.077807, -1.077665, -1.077482, -1.077257, -1.076988, -1.076674, -1.076312, -1.0759, -1.075435, -1.074914, -1.074332, -1.073686, -1.072969, -1.072176, -1.071298, -1.070328, -1.069249, -1.067973])
assert test.like(scale.value, [6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514, 6.018514])
class Optimizer():
def __init__(self):
# Queue for data thread safe
self.data = queue.LifoQueue()
self.control_data = queue.LifoQueue()
# Create thread lock
# self.lock = threading.Lock()
# stop thread variable
self.stop = False
# variable to prevent multiple solutions form running
self.solving = False
# Create thread parameters
self.MPC_thread = threading.Thread(target=self.run, args=(), daemon=True)
# Controller state to pass into get_output function
self.controllerState = con.Controller()
# Optimal Control data updated by thread
self.u_pitch_star = 0.0
self.u_star = 0.0
# time.sleep(1)
# self.MPC_queue.put('worked')
# Initialize optimization parameters
self.initialize_optimization()
def initialize_optimization(self):
################# AIRBORNE OPTIMIZER SETTINGS################ NOW IN 3D
self.m = GEKKO(remote=False) # Airborne optimzer
nt = 11
self.m.time = np.linspace(0, 1, nt)
# options
# self.m.options.NODES = 3
self.m.options.SOLVER = 3
self.m.options.IMODE = 6# MPC mode
# m.options.IMODE = 9 #dynamic ode sequential
self.m.options.MAX_ITER = 200
self.m.options.MV_TYPE = 0
self.m.options.DIAGLEVEL = 0
# final time
self.tf = self.m.FV(value=1.0,lb=0.1,ub=100.0)
# tf = m.FV(value=5.0)
self.tf.STATUS = 1
# Scaled time for Rocket league to get proper time
self.ts = np.multiply(self.m.time, self.tf)
# some constants
self.g = 650 # Gravity (my data says its more like 670)
# v_end = 500
# force (thruster)
self.u_thrust = self.m.FV(value=1.0,lb=0.0,ub=1.0) #Fixed variable, stays on entire time
self.u_thrust = self.m.MV(value=1, lb=0, ub=1) # Manipulated variable non integaer
# self.u_thrust = self.m.MV(value=0,lb=0,ub=1, integer=True) #Manipulated variable integer type
self.u_thrust.STATUS = 1
self.u_thrust.DCOST = 1e-5
# angular acceleration for all 3 axes
self.u_pitch = self.m.MV(value=0.0, lb=-1.0, ub=1.0)
self.u_pitch.STATUS = 1
self.u_pitch.DCOST = 1e-5
self.u_roll = self.m.MV(value=0.0, lb=-1.0, ub=1.0)
self.u_roll.STATUS = 1
self.u_roll.DCOST = 1e-5
self.u_yaw = self.m.MV(value=0.0, lb=-1.0, ub=1.0)
self.u_yaw.STATUS = 1
self.u_yaw.DCOST = 1e-5
self.Tr = -36.07956616966136; # torque coefficient for roll
self.Tp = -12.14599781908070; # torque coefficient for pitch
self.Ty = 8.91962804287785; # torque coefficient for yaw
self.Dr = -4.47166302201591; # drag coefficient for roll
self.Dp = -2.798194258050845; # drag coefficient for pitch
self.Dy = -1.886491900437232; # drag coefficient for yaw
# # integral over time for u^2
# self.u2 = self.m.Var(value=0.0)
# self.m.Equation(self.u2.dt() == 0.5*self.u_thrust**2)
#
# # integral over time for u_pitch^2
# # self.u2_pitch = self.m.Var(value=0)
# # self.m.Equation(self.u2.dt() == 0.5*self.u_pitch**2)
# end time variables to multiply u2 by to get total value of integral
self.p = np.zeros(nt)
self.p[-1] = 1.0
self.final = self.m.Param(value = self.p)
#################GROUND DRIVING OPTIMIZER SETTTINGS##############
self.d = GEKKO(remote=False) # Driving on ground optimizer
ntd = 9
self.d.time = np.linspace(0, 1, ntd) # Time vector normalized 0-1
# options
# self.d.options.NODES = 2
self.d.options.SOLVER = 1
self.d.options.IMODE = 6# MPC mode
# self.d.options.IMODE = 9 #dynamic ode sequential
self.d.options.MAX_ITER = 500
self.d.options.MV_TYPE = 0
self.d.options.DIAGLEVEL = 0
# final time for driving optimizer
self.tf_d = self.d.FV(value=1.0,lb=0.1,ub=100.0)
# allow gekko to change the tfd value
self.tf_d.STATUS = 1
# Scaled time for Rocket league to get proper time
# Boost variable, its integer type since it can only be on or off
self.u_thrust_d = self.d.MV(integer = True, lb=0,ub=1) #Manipulated variable integer type
self.u_thrust_d.STATUS = 1
# self.u_thrust_d.DCOST = 1e-5
# Throttle value, this can vary smoothly between 0-1
self.u_throttle_d = self.d.MV(value = 1, lb = 0.1, ub = 1)
self.u_throttle_d.STATUS = 1
self.u_throttle_d.DCOST = 1e-5
# Turning input value also smooth
self.u_turning_d = self.d.MV(value = 0, lb = -1, ub = 1)
self.u_turning_d.STATUS = 1
self.u_turning_d.DCOST = 1e-5
# end time variables to multiply u2 by to get total value of integral
self.p_d = np.zeros(ntd)
self.p_d[-1] = 1.0
self.final_d = self.d.Param(value = self.p_d)
def MPC_optimize(self, car, ball):
#NOTE: I should make some data structures to easily pass this data around as one variable instead of so many variables
# variables intial conditions are placed here
# CAR VARIABLES
# NOTE: maximum velocites, need to be total velocity magnitude, not max on indididual axes, as you can max on both axes but actually be above the true max velocity of the game
#--------------------------------
# Position of car vector
self.s = self.m.Array(self.m.Var,(3))
ig = [car.x,car.y, car.z]
#Initialize values for each array element
i = 0
for xi in self.s:
xi.value = ig[i]
xi.lower = -2300.0
xi.upper = 2300
i += 1
#--------------------------------
#velocity of car vector
self.v = self.m.Array(self.m.Var,(3))
ig = [car.vx,car.vy, car.vz]
#Initialize values for each array element
i = 0
for xi in self.v:
xi.value = ig[i]
xi.lower = -2300.0
xi.upper = 2300
i += 1
# Pitch rotation and angular velocity
# self.roll = self.m.Var(value = car.roll)
# self.pitch = self.m.Var(value = car.pitch) #orientation pitch angle
# self.yaw = self.m.Var(value = car.yaw)
# self.omega_roll = self.m.Var(value = car.wx, lb = -5.5, ub = 5.5)
# self.omega_pitch = self.m.Var(value=car.wy, lb=-5.5, ub=5.5) #angular velocity
# self.omega_yaw = self.m.Var(value = car.wz, lb = -5.5, ub = 5.5)
#--------------------------------
#Orientation Quaternion
self.q = self.m.Array(self.m.Var, (4))
q = euler_to_quaternion(car.roll, car.pitch, car.yaw)
ig = [q[0], q[1], q[2], q[3]]
#Initialize values for each array element
i = 0
for xi in self.q:
xi.value = ig[i]
i += 1
#--------------------------------
#Angular Velocity quaternion
self.q_dt = self.m.Array(self.m.Var, (4))
#Initialize values for each array element
ig = [0, car.wx, car.wy, car.wz]
#Initialize values for each array element
i = 0
for xi in self.q_dt:
xi.value = ig[i]
i += 1
#--------------------------------
#Thrust direction vector from quaternion
self.thrust_direction_x = self.m.Var(value = get_thrust_direction_x(self.q))
self.thrust_direction_y = self.m.Var(value = get_thrust_direction_y(self.q))
self.thrust_direction_z = self.m.Var(value = get_thrust_direction_z(self.q))
#Rworld_to_car
# r = self.roll #rotation around roll axis to get world to car frame
# p = self.pitch #rotation around pitch axis to get world to car frame
# y = -1*self.yaw #rotation about the world z axis to get world to car frame
# self.Rx = np.matrix([[1, 0, 0], [0, math.cos(r), -1*math.sin(r)], [0, math.sin(r), math.cos(r)]])
# self.Ry = np.matrix([[math.cos(p), 0, math.sin(p)], [0, 1, 0], [-1*math.sin(p), 0, math.cos(p)]])
# self.Rz = np.matrix([[math.cos(y), -1*math.sin(y), 0], [math.sin(y), math.cos(y), 0], [0, 0, 1]])
# #Order of rotations from world to car is x then y then z
# self.Rinter = np.matmul(self.Rx, self.Ry)
# self.Rworld_to_car = np.matmul(self.Rinter, self.Rz)
# self.q = Quaternion(matrix = self.Rworld_to_car).normalised #orientation quaternion created from rotation matrix derived from euler qngle "sensor"
# self.qi = self.q.inverse
# BALL VARIABLES
# NOTE: same issue with max velocity as car, will fix later
self.ball_s = self.m.Array(self.m.Var,(3)) #Ball position
self.ball_s[0].value = ball.x
self.ball_s[1].value = ball.y
self.ball_s[2].value = ball.z
self.ball_v = self.m.Array(self.m.Var,(3)) #Ball Velocity
self.ball_v[0].value = ball.vx
self.ball_v[1].value = ball.vy
self.ball_v[2].value = ball.vz
# differential equations scaled by tf
thrust = np.array([991.666+66.666, 0, 0]) #Thrust lies along the car's local x axis only, so need to rotate this by quaternio car is rotated by to get thrust in each axis
q = np.array([self.q[0],self.q[1],self.q[2]])
# qi = np.array([self.qi[0],self.qi[1],self.qi[2]])
# print(self.q.rotate(t))
# self.thrust_direction = self.m.Array(self.m.Intermediate(self.u_thrust.value * self.q[0]))
# self.thrust_direction_y = self.m.Intermediate(self.q.rotate(thrust)[1])
# self.thrust_direction_z = self.m.Intermediate(self.q.rotate(t)[2]) #Intermediate thrust direction vector rotated by the quaternion
# CARS DIFFERENTIAL EQUATIONS
#angular orientatio quaternion and angular velocity quaternion
#position and velocity
self.m.Equation(self.s[0].dt()==self.tf * self.v[0])
self.m.Equation(self.s[1].dt()==self.tf * self.v[1])
self.m.Equation(self.s[2].dt()==self.tf * self.v[2])
self.m.Equation(self.v[0].dt()==self.tf * ((self.u_thrust*self.thrust_direction_x)))
self.m.Equation(self.v[1].dt()==self.tf * ((self.u_thrust*self.thrust_direction_y)))
self.m.Equation(self.v[2].dt()==self.tf * ((self.u_thrust*self.thrust_direction_z)))
# self.m.Equation(self.pitch.dt()==self.tf * self.omega_pitch)
# self.m.Equation(self.omega_pitch.dt()== self.tf * ((self.u_pitch*self.Tp) + (self.omega_pitch*self.Dp*(1.0-self.m.sqrt(self.u_pitch*self.u_pitch)))))
# BALLS DIFFERENTIAL EQUATIONS
self.m.Equation(self.ball_s[0].dt()==self.tf * self.ball_v[0])
self.m.Equation(self.ball_s[1].dt()==self.tf * self.ball_v[1])
self.m.Equation(self.ball_s[2].dt()==self.tf * self.ball_v[2])
self.m.Equation(self.ball_v[0].dt()==self.tf * 0)
self.m.Equation(self.ball_v[1].dt()==self.tf * 0)
self.m.Equation(self.ball_v[2].dt()==self.tf * (-1.0*self.g))
# self.m.Equation(self.error == self.sx - trajectory_sx)
# hard constraints
# self.m.fix(self.sz, pos = len(self.m.time) - 1, val = 1000)
#Soft constraints for the end point
# Uncomment these 4 objective functions to get a simlple end point optimization
#sf[1] is z position @ final time etc...
# self.m.Obj(self.final*1e3*(self.sz-sf[1])**2) # Soft constraints
# self.m.Obj(self.final*1e3*(self.vz-vf[1])**2)
# self.m.Obj(self.final*1e3*(self.sx-sf[0])**2) # Soft constraints
# self.m.Obj(self.final*1e3*(self.vx-vf[0])**2)
# Objective values to hit into ball in minimal time
self.m.Obj(self.final*1e4*(self.s[2]-self.ball_s[2])**2) # Soft constraints
self.m.Obj(self.final*1e4*(self.s[1]-self.ball_s[1])**2) # Soft constraints
self.m.Obj(self.final*1e4*(self.s[0]-self.ball_s[0])**2) # Soft constraints
# Objective funciton to hit with a particular velocity
# self.m.Obj(self.final*1e3*(self.vz/)**2)
# self.m.Obj(self.final*1e4*(self.vx + 1000)**2)
#Objective function to minimize time
self.m.Obj(self.tf * 1e3)
#Objective functions to follow trajectory
# self.m.Obj(self.final * (self.errorx **2) * 1e3)
# self.m.Obj(self.final*1e3*(self.sx-traj_sx)**2) # Soft constraints
# self.m.Obj(self.errorz)
# self.m.Obj(( self.all * (self.sx - trajectory_sx) **2) * 1e3)
# self.m.Obj(((self.sz - trajectory_sz)**2) * 1e3)
# minimize thrust used
# self.m.Obj(self.u2*self.final*1e3)
# minimize torque used
# self.m.Obj(self.u2_pitch*self.final)
#solve
# self.m.solve('http://127.0.0.1') # Solve with local apmonitor server
self.m.solve()
# NOTE: another data structure type or class here for optimal control vectors
# Maybe it should have some methods to also make it easier to parse through the control vector etc...
# print('time', np.multiply(self.m.time, self.tf.value[0]))
# time.sleep(3)
# self.ts = np.multiply(self.m.time, self.tf.value[0])
# print('ts', self.ts)
# print('ustar', self.u_pitch.value)
# time.sleep(0.10)
return self.u_thrust, self.u_pitch#, self.ts, self.sx, self.sz, self.ball_sx, self.ball_sz, self.ball_vz, self.pitch
def run(self):
while(self.stop == False):
try:
print('in run function')
[car, car_desired] = self.data.get()
print('car actual', car.position, 'car desired', car_desired.position)
if(car != None and self.solving == False):
self.solving = True
self.optimizeDriving(copy.deepcopy(car), copy.deepcopy(car_desired))
print('t', self.ts_d, 'u_turn', self.u_turning_d.value)
#Push data to control data queue
self.control_data.put([self.ts_d, self.u_throttle_d, self.u_turning_d, self.u_thrust_d])
# Save local control vector and time that the control vector should start
except Exception as e:
print('Exception in optimization thread', e)
traceback.print_exc()
class Brain():
def __init__(self, remote=True, bfgs=True, explicit=True):
self.m = GEKKO(remote=remote)
#generic model options
self.m.options.MAX_ITER = 4000
self.m.options.OTOL = 1e-4
self.m.options.RTOL = 1e-4
if bfgs:
self.m.solver_options = ['hessian_approximation limited-memory']
self._explicit = explicit
self._input_size = None
self._output_size = None
self._layers = []
self._weights = []
self._biases = []
self.input = []
self.output = []
def input_layer(self, size):
#store input size
self._input_size = size
#build FV with Feedback to accept inputs
self.input = [self.m.Param() for _ in range(size)]
# #set FV options
# for n in self.input:
# n.FSTATUS = 1
# n.STATUS = 0
#add input layer to list of layers
self._layers.append(self.input)
def layer(self,
linear=0,
relu=0,
tanh=0,
gaussian=0,
bent=0,
leaky=0,
ltype='dense'):
"""
Layer types:
dense
convolution
pool (mean)
Activation options:
none
softmax
relu
tanh
sigmoid
linear
"""
size = relu + tanh + linear + gaussian + bent + leaky
if size < 1:
raise Exception("Need at least one node")
if ltype == 'dense':
## weights between neurons
n_p = len(self._layers[-1]) #number of neuron in previous layer
n_c = n_p * size # number of axion connections
# build n_c FVs as axion weights, initialize randomly in [-1,1]
self._weights.append([
self.m.FV(value=[np.random.rand() * 2 - 1]) for _ in range(n_c)
])
for w in self._weights[-1]:
w.STATUS = 1
w.FSTATUS = 0
#input times weights, add bias and activate
self._biases.append([self.m.FV(value=0) for _ in range(size)])
for b in self._biases[-1]:
b.STATUS = 1
b.FSTATUS = 0
count = 0
if self._explicit:
# build new neuron weighted inputs
neuron_inputs = [
self.m.Intermediate(self._biases[-1][i] + sum(
(self._weights[-1][(i * n_p) + j] *
self._layers[-1][j]) for j in range(n_p)))
for i in range(size)
] #i counts nodes in this layer, j counts nodes of previous layer
##neuron activation
self._layers.append([])
if linear > 0:
self._layers[-1] += [
self.m.Intermediate(neuron_inputs[i])
for i in range(count, count + linear)
]
count += linear
if tanh > 0:
self._layers[-1] += [
self.m.Intermediate(self.m.tanh(neuron_inputs[i]))
for i in range(count, count + tanh)
]
count += tanh
if relu > 0:
self._layers[-1] += [
self.m.Intermediate(
self.m.log(1 + self.m.exp(neuron_inputs[i])))
for i in range(count, count + relu)
]
count += relu
if gaussian > 0:
self._layers[-1] += [
self.m.Intermediate(self.m.exp(-neuron_inputs[i]**2))
for i in range(count, count + gaussian)
]
count += gaussian
if bent > 0:
self._layers[-1] += [
self.m.Intermediate(
(self.m.sqrt(neuron_inputs[i]**2 + 1) - 1) / 2 +
neuron_inputs[i])
for i in range(count, count + bent)
]
count += bent
if leaky > 0:
s = [self.m.Var(lb=0) for _ in range(leaky * 2)]
self.m.Equations([
(1.5 * neuron_inputs[i + count]) -
(0.5 * neuron_inputs[i + count]) == s[2 * i] -
s[2 * i + 1] for i in range(leaky)
])
self._layers[-1] += [
self.m.Intermediate(neuron_inputs[count + i] +
s[2 * i]) for i in range(leaky)
]
[self.m.Obj(s[2 * i] * s[2 * i + 1]) for i in range(leaky)]
self.m.Equations(
[s[2 * i] * s[2 * i + 1] == 0 for i in range(leaky)])
count += leaky
else: #type=implicit
# build new neuron weighted inputs
neuron_inputs = [self.m.Var() for i in range(size)]
self.m.Equations(
[
neuron_inputs[i] == self._biases[-1][i] + sum(
(self._weights[-1][(i * n_p) + j] *
self._layers[-1][j]) for j in range(n_p))
for i in range(size)
]
) #i counts nodes in this layer, j counts nodes of previous layer
##neuron activation
neurons = [self.m.Var() for i in range(size)]
self._layers.append(neurons)
##neuron activation
if linear > 0:
self.m.Equations([
neurons[i] == neuron_inputs[i]
for i in range(count, count + linear)
])
count += linear
if tanh > 0:
self.m.Equations([
neurons[i] == self.m.tanh(neuron_inputs[i])
for i in range(count, count + tanh)
])
for n in neurons[count:count + tanh]:
n.LOWER = -5
n.UPPER = 5
count += tanh
if relu > 0:
self.m.Equations([
neurons[i] == self.m.log(1 +
self.m.exp(neuron_inputs[i]))
for i in range(count, count + relu)
])
for n in neurons[count:count + relu]:
n.LOWER = -10
count += relu
if gaussian > 0:
self.m.Equations([
neurons[i] == self.m.exp(-neuron_inputs[i]**2)
for i in range(count, count + gaussian)
])
for n in neurons[count:count + gaussian]:
n.LOWER = -3.5
n.UPPER = 3.5
count += gaussian
if bent > 0:
self.m.Equations([
neurons[i] == (
(self.m.sqrt(neuron_inputs[i]**2 + 1) - 1) / 2 +
neuron_inputs[i])
for i in range(count, count + bent)
])
count += bent
if leaky > 0:
s = [self.m.Var(lb=0) for _ in range(leaky * 2)]
self.m.Equations([
(1.5 * neuron_inputs[count + i]) -
(0.5 * neuron_inputs[count + i]) == s[2 * i] -
s[2 * i + 1] for i in range(leaky)
])
self.m.Equations([
neurons[count + i] == neuron_inputs[count + i] +
s[2 * i] for i in range(leaky)
])
[
self.m.Obj(10000 * s[2 * i] * s[2 * i + 1])
for i in range(leaky)
]
#self.m.Equations([s[2*i]*s[2*i+1] == 0 for i in range(leaky)])
count += leaky
else:
raise Exception('layer type not implemented yet')
def output_layer(self, size, ltype='dense', activation='linear'):
"""
Layer types:
dense
convolution
pool (mean)
Activation options:
none
softmax
relu
tanh
sigmoid
linear
"""
# build a layer to ensure that the number of nodes matches the output
self.layer(size, 0, 0, 0, 0, 0, ltype)
self.output = [self.m.CV() for _ in range(size)]
for o in self.output:
o.FSTATUS = 1
o.STATUS = 1
#link output CVs to last layer
for i in range(size):
self.m.Equation(self.output[i] == self._layers[-1][i])
def think(self, inputs):
#convert inputs to numpy ndarray
inputs = np.atleast_2d(inputs)
##confirm input/output dimensions
in_dims = inputs.shape
ni = len(self.input)
#consistent layer size
if in_dims[0] != ni:
raise Exception('Inconsistent number of inputs')
#set input values
for i in range(ni):
self.input[i].value = inputs[i, :]
#solve in SS simulation
self.m.options.IMODE = 2
#disable all weights
for wl in self._weights:
for w in wl:
w.STATUS = 0
for bl in self._biases:
for b in bl:
b.STATUS = 0
self.m.solve(disp=False)
##return result
res = [] #concatentate result from each CV in one list
for i in range(len(self.output)):
res.append(self.output[i].value)
return res
def learn(self, inputs, outputs, obj=2, gap=0, disp=True):
"""
Make the brain learn.
Give inputs as (n)xm
Where n = input layer dimensions
m = number of datasets
Give outputs as (n)xm
Where n = output layer dimensions
m = number of datasets
Objective can be 1 (L1 norm) or 2 (L2 norm)
If obj=1, gap provides a deadband around output matching.
"""
#convert inputs to numpy ndarray
inputs = np.atleast_2d(inputs)
outputs = np.atleast_2d(outputs)
##confirm input/output dimensions
in_dims = inputs.shape
out_dims = outputs.shape
ni = len(self.input)
no = len(self.output)
#consistent dataset size
if in_dims[1] != out_dims[1]:
raise Exception('Inconsistent number of datasets')
#consistent layer size
if in_dims[0] != ni:
raise Exception('Inconsistent number of inputs')
if out_dims[0] != no:
raise Exception('Inconsistent number of outputs')
#set input values
for i in range(ni):
self.input[i].value = inputs[i, :]
#set output values
for i in range(no):
o = self.output[i]
o.value = outputs[i, :]
if obj == 1: #set meas_gap while cycling through CVs
o.MEAS_GAP = gap
#solve in MPU mode
self.m.options.IMODE = 2
self.m.options.EV_TYPE = obj
self.m.options.REDUCE = 3
#enable all weights
for wl in self._weights:
for w in wl:
w.STATUS = 1
for bl in self._biases:
for b in bl:
b.STATUS = 1
self.m.solve(disp=disp)
def shake(self, percent):
""" Neural networks are non-convex. Some stochastic shaking can
sometimes help bump the problem to a new region. This function
perturbs all weights by +/-percent their values."""
for l in self._weights:
for f in l:
f.value = f.value[-1] * (
1 + (1 - 2 * np.random.rand()) * percent / 100)
lkr = [
3,
np.log10(0.1),
np.log10(2e-7),
np.log10(0.5),
np.log10(5),
np.log10(100)
]
#%% Model
m = GEKKO()
#time
m.time = np.linspace(0, 15, 61)
#parameters to estimate
lg10_kr = [m.FV(value=lkr[i]) for i in range(6)]
#variables
kr = [m.Var() for i in range(6)]
H = m.Var(value=1e6)
I = m.Var(value=0)
V = m.Var(value=1e2)
#Variable to match with data
LV = m.CV(value=2)
#equations
m.Equations([10**lg10_kr[i] == kr[i] for i in range(6)])
m.Equations([
H.dt() == kr[0] - kr[1] * H - kr[2] * H * V,
I.dt() == kr[2] * H * V - kr[3] * I,
V.dt() == -kr[2] * H * V - kr[4] * V + kr[5] * I, LV == m.log10(V)
])
gek = GEKKO()
# set up the time
# --------------------------------------
num_points = 70
max_time = 300
gek.time = np.linspace(0, 1, num_points)
# MV Initialization
# --------------------------------------
G = gek.MV(value=0, lb=0)
Fb = gek.MV(value=0, lb=0)
# set up the time variable (to minimize)
# --------------------------------------
tf = gek.FV(value=120, lb=0, ub=max_time)
# turn them 'on'
# --------------------------------------
for s in (G, Fb, tf):
s.STATUS = 1
# Variable Initialization
# --------------------------------------
x = gek.Var(value=0, lb=0, ub=v_goal) # Position (miles)
v = gek.Var(value=0, lb=0, ub=speed_limit) # Velocity
a = gek.Var(value=0, ub=3, lb=-3) # Acceleration
Fe = gek.Var(value=0) # Fuel Efficiency
Fd = gek.Var(value=0) # Fuel Consumption(?)
FUEL = gek.Var(value=0)
from scipy import optimize
import matplotlib.pyplot as plt
from gekko import GEKKO
import numpy as np
m = GEKKO()
m.options.SOLVER = 3
m.options.IMODE = 2
xzd = np.linspace(1,5,100)
yzd = np.sin(xzd)
xz = m.Param(value=xzd)
yz = m.CV(value=yzd)
yz.FSTATUS = 1
xp_val = np.array([1, 2, 3, 3.5, 4, 5])
yp_val = np.array([1, 0, 2, 2.5, 2.8, 3])
xp = [m.FV(value=xp_val[i],lb=xp_val[0],ub=xp_val[-1]) for i in range(6)]
yp = [m.FV(value=yp_val[i]) for i in range(6)]
for i in range(6):
xp[i].STATUS = 0
yp[i].STATUS = 1
for i in range(5):
m.Equation(xp[i+1]>=xp[i]+0.05)
x = [m.Var(lb=xp[i],ub=xp[i+1]) for i in range(5)]
x[0].lower = -1e20
x[-1].upper = 1e20
# Variables
slk_u = [m.Var(value=1,lb=0) for i in range(4)]
slk_l = [m.Var(value=1,lb=0) for i in range(4)]
# Intermediates
slope = []
for i in range(5):
class solveMPC:
def __init__(self, env):
self.theta_goal = 70
self.thetadot_goal = 0
self.x_goal = env.X / 2
self.y_goal = env.Y
self.last_u = 0 #Keep track of last input control
def action(self, state, plot=False, integer_control=False):
#Return a discrete action 0-21 based on the dynamic optimization solution.
"""
State Variables:
These are the variables included in the dynamics
Manipulated Variables:
These are the control inputs
"""
# Initialize the GEKKO solver and set options:
# The remote argument allows for the solver to be run on Brigham Young Universitie's public server
self.m = GEKKO(remote=True)
init_state = state
self.m.options.IMODE = 6 # control
if integer_control:
self.m.options.SOLVER = 1 #Use APOPT Solver (only solver able to handle mixed integer)
else:
self.m.options.SOLVER = 3 #Use IPOPT Solver
#Setup the solvers discretization array:
# disc_length is the total number of discretization steps for the solver to discretize the problem
disc_len = 501
self.disc_array = np.linspace(
0, 1, disc_len
) #Solver time steps (For variable final time, this will be scaled)
self.m.time = self.disc_array
#Fixed Varaibles: These are fixed over the horizon but able to change for each iteration
#tf_scale is the final time variable. This is a trick to get the solver to solve a free final time problem
self.tf_scale = self.m.FV(value=150, lb=1, ub=2000)
self.tf_scale.STATUS = 1
# Parameters of environment (state space model)
tau = self.m.Param(value=90)
ku = self.m.Param(value=18)
dist = self.m.Param(value=0)
# Manipulated variables (Control Input)
u = self.m.MV(value=self.last_u,
lb=-10,
ub=10,
integer=integer_control)
u_step_len = 30 #feet; the length overwhich control must stay constant
u.MV_STEP_HOR = self.get_control_step(u_step_len, ku.value, state[2],
self.theta_goal, disc_len)
u.STATUS = 1 # allow optimizer to change u
#Handle Units
#States have the following units below: x:feet y:feet theta:deg thetadot:deg/ft
#Convert units of states (1,1, deg->rad *Acutally ode is in deg, deg/100ft -> deg/ft)
#State Input: x y thetao thetadoto
convert_units = np.array([1, 1, 1, 1 / 100])
init_state = init_state * convert_units
d2r = np.pi / 180
# State Variables
x = self.m.SV(value=init_state[0], lb=-10000, ub=10000)
y = self.m.SV(value=init_state[1], lb=-10000, ub=10000)
theta = self.m.SV(value=init_state[2], lb=-20, ub=100)
thetadot = self.m.SV(value=init_state[3], lb=-.3, ub=.3)
thetadotmax = self.m.SV(value=ku.value, lb=-.3, ub=.3)
# Initialize binary vector used to retreive the final state ( vector of all zeros except last element = 1 )
final = self.m.Param(np.zeros(self.m.time.shape[0]))
final.value[-1] = 1
# Dynamics
self.m.Equation(theta.dt() == thetadot * self.tf_scale)
self.m.Equation(
thetadot.dt() == -(1 / tau) * thetadot * self.tf_scale +
(.01 * ku / tau) * u / 10 * self.tf_scale +
.01 * dist / tau * self.tf_scale)
self.m.Equation(x.dt() == self.m.cos(theta * d2r) * self.tf_scale)
self.m.Equation(y.dt() == self.m.sin(theta * d2r) * self.tf_scale)
self.m.Equation(
thetadotmax >= thetadot) #This variable acts as max(thetadot)
# Objective Functions
self.m.Obj((thetadotmax)**2)
self.m.Obj(10 * (x * final - self.x_goal)**2)
self.m.Obj(10 * (y * final - self.y_goal)**2)
self.m.Obj((theta * final - self.theta_goal)**2)
self.m.Obj(.1 * (thetadot * final - self.thetadot_goal)**2)
try:
self.m.solve(disp=False)
except:
print('Enter Debug Mode')
if plot:
self.plot(u, x, y, theta, thetadot)
self.last_u = u.value[1]
action = int(round(u.value[1] / 10, 1) * 10 + 10)
return action
def get_control_step(self, desired_u_step, ku, thetao, theta_goal,
disc_len):
"""
This function estimates the number of solver discretization steps that the
control input should stay constant. We have to estimate what this value should
be since the final time is free and the solver doesn't allow us to vary the
control step size with each solver iteration.
First guess the length of the optimal trajectory, then convert this from
feet to discretization steps"""
delta_theta = np.abs(thetao - theta_goal)
curvature_max = ku
#Calculate the average arc length if we were to drill with full curvature
#and half curvature capabilities
avg_arc_length = 3 / 2 * delta_theta / curvature_max * 100
u_solver_step = int(desired_u_step * disc_len / avg_arc_length)
u_solver_step = max(u_solver_step, 1)
return u_solver_step
def plot(self, u, x, y, theta, thetadot):
theta = np.array(theta.value)
thetadot = np.array(thetadot.value)
x = np.array(x.value)
y = np.array(y.value)
sim_t = self.disc_array * self.tf_scale.value[0]
plt.subplot(3, 1, 1)
plt.plot(sim_t, u.value, 'b-', label='Control Optimized')
plt.legend()
plt.ylabel('Input')
plt.subplot(3, 1, 2)
plt.plot(sim_t, theta, 'r--', label='Theta Response')
plt.ylabel('Deg')
plt.legend(loc='best')
plt.subplot(3, 1, 3)
plt.plot(sim_t, thetadot, 'r--', label='Theta_dot Response')
plt.ylabel('Units')
plt.xlabel('Measured Depth (ft)')
plt.legend(loc='best')
plt.show()
plt.plot(y, -x, 'r--', label='Trajectory')
plt.ylabel('TVD')
plt.xlabel('Cross Section')
plt.legend(loc='best')
plt.grid()
plt.show()
T = reaction_model.Param(temperature) # Fixed Variables to change # bounds """ E_a_1 = reaction_model.FV(E_a_1_initial_guess, lb = 33900, ub = 37900) E_a_2 = reaction_model.FV(E_a_2_initial_guess, lb = 73000, ub = 81000) A_1 = reaction_model.FV(A_1_initial_guess, lb = 0) A_2 = reaction_model.FV(A_2_initial_guess, lb = 0) alpha = reaction_model.FV(alpha_initial_guess)#, lb = 0, ub = 1) beta = reaction_model.FV(beta_initial_guess)#, lb = 0, ub = 1) """ # NO bounds E_a_1 = reaction_model.FV(E_a_1_initial_guess) E_a_2 = reaction_model.FV(E_a_2_initial_guess) A_1 = reaction_model.FV(A_1_initial_guess) A_2 = reaction_model.FV(A_2_initial_guess) alpha = reaction_model.FV(alpha_initial_guess) beta = reaction_model.FV(beta_initial_guess) # one-sided bounds #alpha.LOWER = 0 #beta.LOWER = 0 # state Variables Ph_3_minus = reaction_model.SV(Ph_3_minus_initial) # variable we will use to regress other Parameters Ph_2_minus = reaction_model.CV(ph_abs)
def model(T0,t,M,Kp,taus,zeta):
# T0 = initial T
# t = time
# M = magnitude of the step
# Kp = gain
# taus = second order time constant
# zeta = damping factor (zeta>1 for overdamped)
T = ?
return T
# Connect to Arduino
a = tclab.TCLab()
# Second order model of TCLab
m = GEKKO(remote=False)
Kp = m.FV(1.0,lb=0.5,ub=2.0)
taus = m.FV(50,lb=10,ub=200)
zeta = m.FV(1.2,lb=1.1,ub=5)
y0 = a.T1
u = m.MV(0)
x = m.Var(y0); y = m.CV(y0)
m.Equation(x==y.dt())
m.Equation((taus**2)*x.dt()+2*zeta*taus*y.dt()+(y-y0) == Kp*u)
m.options.IMODE = 5
m.options.NODES = 2
m.time = np.linspace(0,200,101)
m.solve(disp=False)
y.FSTATUS = 1
# Turn LED on
print('LED On')
Umhe = 10.0 * np.ones(n) taumhe = 5.0 * np.ones(n) amhe1 = 0.01 * np.ones(n) amhe2 = 0.0075 * np.ones(n) ######################################################### # Initialize Model as Estimator ######################################################### m = GEKKO(name='tclab-mhe') m.server = 'http://127.0.0.1' # if local server is installed # 60 second time horizon, 20 steps m.time = np.linspace(0, 60, 21) # Parameters to Estimate U = m.FV(value=10, name='u') U.STATUS = 0 # don't estimate initially U.FSTATUS = 0 # no measurements U.DMAX = 1 U.LOWER = 5 U.UPPER = 15 tau = m.FV(value=5, name='tau') tau.STATUS = 0 # don't estimate initially tau.FSTATUS = 0 # no measurements tau.DMAX = 1 tau.LOWER = 4 tau.UPPER = 8 alpha1 = m.FV(value=0.01, name='a1') # W / % heater alpha1.STATUS = 0 # don't estimate initially
import matplotlib.pyplot as plt from gekko import GEKKO import numpy as np m = GEKKO(remote=False) m.options.SOLVER = 1 x = m.FV(value=4.5) y = m.Var() xp = np.array([1, 2, 3, 3.5, 4, 5]) yp = np.array([1, 0, 2, 2.5, 2.8, 3]) m.pwl(x, y, xp, yp) m.solve() plt.plot(xp, yp, 'rx-', label='PWL function') plt.plot(x, y, 'bo', label='Data') plt.show()
from gekko import GEKKO import numpy as np import matplotlib.pyplot as plt m = GEKKO(remote=False) xm = np.array([0, 1, 2, 3, 4, 5]) ym = np.array([0.1, 0.2, 0.3, 0.5, 0.8, 2.0]) m.options.IMODE = 2 # coeffs: c = [m.FV(value=0) for i in range(4)] x = m.Param(value=xm) y = m.CV(value=ym) # cv: match the model and the measured value y.FSTATUS = 1 # we gonna use the measurements # polynom model itself m.Equation(y == c[0] + c[1] * x + c[2] * x**2 + c[3] * x**3) # linReg c[0].STATUS = 1 c[1].STATUS = 1 m.options.EV_TYPE = 1 # error is absolute # m.options.EV_TYPE = 2 # error is squarred m.solve(disp=False) p1 = [c[1].value[0], c[0].value[0]] xp = np.linspace(0, 5, 100) c[2].STATUS = 1
mhe_ac.FSTATUS=1 mhe_v = mhe.CV(value=0, name='v',lb=0) #vs mhe_v.FSTATUS=1 mhe_v.STATUS=1 mhe_v.MEAS_GAP=1 mhe_a = mhe.SV(value=0,name='a') #acc mhe_f=mhe.SV(value=0,name='f') mhe_grade=mhe.MV(0) mhe_grade.STATUS=0 mhe_grade.FSTATUS=1 mhe_cd=mhe.FV(value=0.003875,lb=0,ub=50) mhe_rr1=mhe.FV(value=0.001325,lb=0,ub=50) mhe_rr2=mhe.FV(value=0.265,lb=0,ub=50) mhe_tau=mhe.FV(value=0.01,lb=0) mhe_k1=mhe.FV(value=10) mhe_k2=mhe.FV(value=0.2) mhe_tm=mhe.Param(value=360) # total mass mhe_iw=mhe.Param(value=26) # total mass #dmaxs=[10,10,10,] for ind,fixed in enumerate([mhe_cd,mhe_rr1,mhe_rr2,mhe_k1,mhe_k2,mhe_tau]): fixed.STATUS=0 fixed.FSTATUS=0 fixed.DMAX=10 # fixed.LOWER=0 mhe_k1.DMAX=100
#
# Data init
#
data_file = np.loadtxt('GEKKO/Bioreactor/data.txt', delimiter=',')
time = data_file[:, 0]
cells_data = data_file[:, 1]
substrate_data = data_file[:, 2]
o2_data = data_file[:, 3]
co2_data = data_file[:, 4]
# GEKKO model init
m = GEKKO(remote=False)
m.time = time
# parameters
rf = m.FV(value=0.5, lb=0, name='R_F (?)')
rf.STATUS = 1
rsmax = m.FV(value=0.01519, lb=0, name='rs_max (%/g-d)')
rsmax.STATUS = 1
ks = m.FV(value=0.186, lb=0)
ks.STATUS = 1
yxs = m.FV(value=5.60, lb=0)
yxs.STATUS = 1
mum = m.FV(value=0.118, lb=0, name='mu_maintenance (1/d)')
mum.STATUS = 1
romax = m.FV(value=85, lb=0, name="ro_max (%/g-d)")
romax.STATUS = 1
ko = m.FV(value=6.32, lb=0, name="k_o (%)")
ko.STATUS = 1
osat = m.FV(value=86.2, lb=0, name="o_sat (%)")
osat.STATUS = 1
# filtered bias update alpha = 0.0951 # mhe tuning horizon = 30 #%% Model #Initialize model m = GEKKO() #time array m.time = np.arange(50) #Parameters u = m.Param(value=42) d = m.FV(value=0) Cv = m.Param(value=1) tau = m.Param(value=0.1) #Variable flow = m.CV(value=42) #Equation m.Equation(tau * flow.dt() == -flow + Cv * u + d) # Options m.options.imode = 5 m.options.ev_type = 1 #start with l1 norm m.options.coldstart = 1 d.status = 1
E = 1 c = 17.5 r = 0.71 k = 80.5 U_max = 20 u = m.MV(lb=0, ub=1, value=1) u.STATUS = 1 x = m.Var(value=70) m.Equation(x.dt() == r * x * (1 - x / k) - u * U_max) J = m.Var(value=0) Jf = m.FV() Jf.STATUS = 1 m.Connection(Jf, J, pos2='end') m.Equation(J.dt() == (E - c / x) * u * U_max) m.Obj(-Jf) m.options.IMODE = 6 m.options.NODES = 3 m.options.SOLVER = 3 m.solve(debug=True) #m.GUI() print(Jf.value[0]) plt.figure()
def pitch(mission, rocket, stage, trajectory, general, optimization, Data):
alpha_v = 20
g0 = 9.810665
Re = 6371000
T = stage[0].thrust
Mass = rocket.mass
ISP = stage[0].Isp
Area = stage[0].diameter**2 / 4 * np.pi
#The pitch is also considered only in the first stage
m = GEKKO(remote=False)
m.time = np.linspace(0, trajectory.pitch_time, 101)
final = np.zeros(len(m.time))
final[-1] = 1
final = m.Param(value=final)
tf = m.FV(value=1, lb=0.1, ub=100)
tf.STATUS = 0
alpha = m.MV(value=0, lb=-0.2, ub=0.1)
alpha.STATUS = 1
alpha.DMAX = alpha_v * np.pi / 180 * trajectory.pitch_time / 101
x = m.Var(value=Data[1][-1], lb=0)
y = m.Var(value=Data[2][-1], lb=0)
v = m.Var(value=Data[3][-1], lb=0)
phi = m.Var(value=Data[4][-1], ub=np.pi / 2, lb=0)
mass = m.Var(value=Data[5][-1])
vG = m.Var(value=Data[7][-1])
vD = m.Var(value=Data[8][-1])
rho = m.Intermediate(rho_func(y))
cD = m.Intermediate(Drag_Coeff(v / (m.sqrt(1.4 * 287.053 * Temp_func(y)))))
D = m.Intermediate(1 / 2 * rho * Area * cD * v**2)
#D=m.Intermediate(Drag_force(rho_func(y),Temp_func(y),Area,v,1,n))
m.Equations([
x.dt() / tf == v * m.cos(phi),
y.dt() / tf == v * m.sin(phi),
v.dt() / tf == T / mass * m.cos(alpha) - D / mass - g0 * m.sin(phi) *
(Re / (Re + y))**2, ISP * g0 * mass.dt() / tf == -T, v * phi.dt() /
tf == (v**2 / (Re + y) - g0 *
(Re / (Re + y))**2) * m.cos(phi) + T * m.sin(alpha) / mass,
vG.dt() / tf == g0 * m.sin(phi) * (Re / (Re + y))**2,
vD.dt() / tf == D / mass
])
#m.fix(alpha,100,0)
m.Obj(final * (phi - trajectory.pitch)**2)
m.Obj(1e-4 * alpha**2)
m.options.IMODE = 6
m.options.SOLVER = 3
m.options.MAX_ITER = 500
m.solve(disp=False)
###Save Data
tm = np.linspace(Data[0][-1], m.time[-1] + Data[0][-1], 101)
Data[0].extend(tm[1:])
Data[1].extend(x.value[1:])
Data[2].extend(y.value[1:])
Data[3].extend(v.value[1:])
Data[4].extend(phi.value[1:])
Data[5].extend(mass.value[1:])
Data[6].extend(alpha.value[1:])
Data[7].extend(vG.value[1:])
Data[8].extend(vD.value[1:])
print(Data[4][-1])
return Data
class TrajectoryGenerator():
def __init__(self):
#################GROUND DRIVING OPTIMIZER SETTTINGS##############
self.d = GEKKO(remote=False) # Driving on ground optimizer
ntd = 7
self.d.time = np.linspace(0, 1, ntd) # Time vector normalized 0-1
# options
# self.d.options.NODES = 3
self.d.options.SOLVER = 3
self.d.options.IMODE = 6 # MPC mode
# m.options.IMODE = 9 #dynamic ode sequential
self.d.options.MAX_ITER = 200
self.d.options.MV_TYPE = 0
self.d.options.DIAGLEVEL = 0
# final time for driving optimizer
self.tf = self.d.FV(value=1.0, lb=0.1, ub=100.0)
# allow gekko to change the tfd value
self.tf.STATUS = 1
# Scaled time for Rocket league to get proper time
# Acceleration variable
self.a = self.d.MV(value=1.0, lb=0.0, ub=1.0, integer=True)
self.a.STATUS = 1
self.a.DCOST = 1e-10
# # Boost variable, its integer type since it can only be on or off
# self.u_thrust_d = self.d.MV(value=0,lb=0,ub=1, integer=False) #Manipulated variable integer type
# self.u_thrust_d.STATUS = 0
# self.u_thrust_d.DCOST = 1e-5
#
# # Throttle value, this can vary smoothly between 0-1
# self.u_throttle_d = self.d.MV(value = 1, lb = 0.02, ub = 1)
# self.u_throttle_d.STATUS = 1
# self.u_throttle_d.DCOST = 1e-5
# Turning input value also smooth
# self.u_turning_d = self.d.MV(lb = -1, ub = 1)
# self.u_turning_d.STATUS = 1
# self.u_turning_d.DCOST = 1e-5
# end time variables to multiply u2 by to get total value of integral
self.p_d = np.zeros(ntd)
self.p_d[-1] = 1.0
self.final = self.d.Param(value=self.p_d)
# integral over time for u_pitch^2
# self.u2_pitch = self.d.Var(value=0)
# self.d.Equation(self.u2.dt() == 0.5*self.u_pitch**2)
# Data for generating a trajectory
self.initial_car_state = Car()
self.final_car_state = Car()
def update_from_packet(self, packet, idx): # update initial car state
data = packet.game_cars[idx]
#update stuff from packet
self.initial_car_state.update(data)
def update_final_car_state(self, data):
self.final_car_state.update(data)
def optimize2D(self, si, sf, vi, vf, ri,
omegai): #these are 1x2 vectors s or v [x, z]
#NOTE: I should make some data structures to easily pass this data around as one variable instead of so many variables
#Trajectory to follow
w = 0.5 # radians/sec of rotation
amp = 100 #amplitude
traj_sx = amp * self.d.cos(w * self.d.time)
# self.t_sx = self.d.Var(value = amp) # Pre-Defined trajectory
# self.d.Equation(self.t_sx.dt() == w * amp * self.d.sin(w * self.d.time))
# self.t_sz = self.d.Var(value = 100) # Pre-Defined trajectory
# self.d.Equation(self.t_sz.dt() == -1 * w * amp * self.d.cos(w * self.d.time))
# variables intial conditions are placed here
# Position and Velocity in 2d
self.sx = self.d.Var(value=si[0], lb=-4096, ub=4096) #x position
# self.vx = self.d.Var(value=vi[0]) #x velocity
self.sy = self.d.Var(value=si[1], lb=-5120, ub=5120) #y position
# self.vy = self.d.Var(value=vi[1]) #y velocity
# Pitch rotation and angular velocity
self.yaw = self.d.Var(value=ri) #orientation yaw angle
# self.omega_yaw = self.d.Var(value=omegai, lb=-5.5, ub=5.5) #angular velocity
# self.v_mag = self.d.Intermediate(self.d.sqrt((self.vx**2) + (self.vy**2)))
self.v_mag = self.d.Var(value=self.d.sqrt((vi[0]**2) + (vi[1]**2)),
ub=2500)
self.curvature = self.d.Intermediate(
(0.0069 - ((7.67e-6) * self.v_mag) + ((4.35e-9) * self.v_mag**2) -
((1.48e-12) * self.v_mag**3) + ((2.37e-16) * self.v_mag**4)))
self.vx = self.d.Intermediate(self.v_mag * self.d.cos(self.yaw))
self.vy = self.d.Intermediate(self.v_mag * self.d.sin(self.yaw))
# Errors to minimize trajectory error, take the derivative to get the final error value
# self.errorx = self.d.Var(value = 0)
# self.d.Equation(self.errorx.dt() == 0.5*(self.sx - self.t_sx)**2)
# Differental equations
self.d.Equation(self.v_mag.dt() == self.tf * self.a * (991.666 + 60))
self.d.Equation(self.sx.dt() == self.tf *
((self.v_mag * self.d.cos(self.yaw))))
self.d.Equation(self.sy.dt() == self.tf *
((self.v_mag * self.d.sin(self.yaw))))
# self.d.Equation(self.vx.dt() == self.tf * (self.a * (991.666+60) * self.d.cos(self.yaw)))
# self.d.Equation(self.vy.dt() == self.tf * (self.a * (991.666+60) * self.d.sin(self.yaw)))
# self.d.Equation(self.vx.dt()==self.tf *(self.a * ((-1600 * self.v_mag/1410) +1600) * self.d.cos(self.yaw)))
# self.d.Equation(self.vy.dt()==self.tf *(self.a * ((-1600 * self.v_mag/1410) +1600) * self.d.sin(self.yaw)))
# self.d.Equation(self.vx == self.tf * (self.v_mag * self.d.cos(self.yaw)))
# self.d.Equation(self.vy == self.tf * (self.v_mag * self.d.sin(self.yaw)))
self.d.Equation(self.yaw.dt() <= self.tf *
(self.curvature * self.v_mag))
# self.d.fix(self.sz, pos = len(self.d.time) - 1, val = 1000)
#Soft constraints for the end point
# Uncomment these 4 objective functions to get a simlple end point optimization
#sf[1] is z position @ final time etc...
self.d.Obj(self.final * 1e4 * (self.sy - sf[1])**2) # Soft constraints
# self.d.Obj(self.final*1e3*(self.vy-vf[1])**2)
self.d.Obj(self.final * 1e4 * (self.sx - sf[0])**2) # Soft constraints
# self.d.Obj(self.final*1e3*(self.vx-vf[0])**2)
#Objective function to minimize time
self.d.Obj(self.tf * 1e5)
#Objective functions to follow trajectory
# self.d.Obj(self.final * (self.errorx **2) * 1e3)
# self.d.Obj(self.final*1e3*(self.sx-traj_sx)**2) # Soft constraints
# self.d.Obj(self.errorz)
# self.d.Obj(( self.all * (self.sx - trajectory_sx) **2) * 1e3)
# self.d.Obj(((self.sz - trajectory_sz)**2) * 1e3)
# minimize thrust used
# self.d.Obj(self.u2*self.final*1e3)
# minimize torque used
# self.d.Obj(self.u2_pitch*self.final)
#solve
# self.d.solve('http://127.0.0.1') # Solve with local apmonitor server
try:
self.d.solve()
except Exception as e:
print('solver error', e)
return None, None, None
# NOTE: another data structure type or class here for optimal control vectors
# Maybe it should have some methods to also make it easier to parse through the control vector etc...
# print('time', np.multiply(self.d.time, self.tf.value[0]))
# time.sleep(3)
self.ts = np.multiply(self.d.time, self.tf.value[0])
return self.a, self.yaw, self.ts
def generateDrivingTrajectory(self, i,
f): #i = initial state, f=final state
# Extract data from initial and fnial states
print('initial state data x', i.x)
if (i.x != None):
s_ti = [i.x, i.y]
v_ti = [i.vx, i.vy]
s_tf = [f.x, f.y]
v_tf = [f.vx, f.vy]
r_ti = i.yaw # inital orientation of the car
omega_ti = i.wz # initial angular velocity of car
else:
return None, None, None, None, None
# Run optimization algorithm
try:
a, theta, t = self.optimize2D(s_ti, s_tf, v_ti, v_tf, r_ti,
omega_ti)
# self.plot_data()
return self.sx, self.sy, self.vx, self.vy, self.yaw
except Exception as e:
print('Error in optimize or plotting', e)
return None, None, None, None, None
def plot_data(self):
# print('u', acceleration.value)
# print('tf', self.tf.value)
# print('tf', self.tf.value[0])
print('sx', self.sx.value)
print('sy', self.sy.value)
print('a', self.a.value)
ts = self.d.time * self.tf.value[0]
# plot results
fig = plt.figure(2)
ax = fig.add_subplot(111, projection='3d')
# plt.subplot(2, 1, 1)
Axes3D.plot(ax, self.sx.value, self.sy.value, ts, c='r', marker='o')
plt.ylim(-5120, 5120)
plt.xlim(-4096, 4096)
plt.ylabel('Position y')
plt.xlabel('Position x')
ax.set_zlabel('time')
fig = plt.figure(3)
ax = fig.add_subplot(111, projection='3d')
# plt.subplot(2, 1, 1)
Axes3D.plot(ax, self.vx.value, self.vy.value, ts, c='r', marker='o')
plt.ylim(-2500, 2500)
plt.xlim(-2500, 2500)
plt.ylabel('velocity y')
plt.xlabel('Velocity x')
ax.set_zlabel('time')
plt.figure(1)
plt.subplot(3, 1, 1)
plt.plot(ts, self.a, 'r-')
plt.ylabel('acceleration')
plt.subplot(3, 1, 2)
plt.plot(ts, np.multiply(self.yaw, 1 / math.pi), 'r-')
plt.ylabel('turning input')
plt.subplot(3, 1, 3)
plt.plot(ts, self.v_mag, 'b-')
plt.ylabel('vmag')
# plt.figure(1)
#
# plt.subplot(7,1,1)
# plt.plot(ts,self.sz.value,'r-',linewidth=2)
# plt.ylabel('Position z')
# plt.legend(['sz (Position)'])
#
# plt.subplot(7,1,2)
# plt.plot(ts,self.vz.value,'b-',linewidth=2)
# plt.ylabel('Velocity z')
# plt.legend(['vz (Velocity)'])
#
# # plt.subplot(4,1,3)
# # plt.plot(ts,mass.value,'k-',linewidth=2)
# # plt.ylabel('Mass')
# # plt.legend(['m (Mass)'])
#
# plt.subplot(7,1,3)
# plt.plot(ts,self.u_thrust.value,'g-',linewidth=2)
# plt.ylabel('Thrust')
# plt.legend(['u (Thrust)'])
#
# plt.subplot(7,1,4)
# plt.plot(ts,self.sx.value,'r-',linewidth=2)
# plt.ylabel('Position x')
# plt.legend(['sx (Position)'])
#
# plt.subplot(7,1,5)
# plt.plot(ts,self.vx.value,'b-',linewidth=2)
# plt.ylabel('Velocity x')
# plt.legend(['vx (Velocity)'])
#
# # plt.subplot(4,1,3)
# # plt.plot(ts,mass.value,'k-',linewidth=2)
# # plt.ylabel('Mass')
# # plt.legend(['m (Mass)'])
#
# plt.subplot(7,1,6)
# plt.plot(ts,self.u_pitch.value,'g-',linewidth=2)
# plt.ylabel('Torque')
# plt.legend(['u (Torque)'])
#
# plt.subplot(7,1,7)
# plt.plot(ts,self.pitch.value,'g-',linewidth=2)
# plt.ylabel('Theta')
# plt.legend(['p (Theta)'])
#
# plt.xlabel('Time')
# plt.figure(2)
#
# plt.subplot(2,1,1)
# plt.plot(self.m.time,m.t_sx,'r-',linewidth=2)
# plt.ylabel('traj pos x')
# plt.legend(['sz (Position)'])
#
# plt.subplot(2,1,2)
# plt.plot(self.m.time,m.t_sz,'b-',linewidth=2)
# plt.ylabel('traj pos z')
# plt.legend(['vz (Velocity)'])
# #export csv
#
# f = open('optimization_data.csv', 'w', newline = "")
# writer = csv.writer(f)
# writer.writerow(['time', 'sx', 'sz', 'vx', 'vz', 'u thrust', 'theta', 'omega_pitch', 'u pitch']) # , 'vx', 'vy', 'vz', 'ax', 'ay', 'az', 'quaternion', 'boost', 'roll', 'pitch', 'yaw'])
# for i in range(len(self.m.time)):
# row = [self.m.time[i], self.sx.value[i], self.sz.value[i], self.vx.value[i], self.vz.value[i], self.u_thrust.value[i], self.pitch.value[i],
# self.omega_pitch.value[i], self.u_pitch.value[i]]
# writer.writerow(row)
# print('wrote row', row)
plt.show()
def free_flight(vA, mission, rocket, stage, trajectory, general, optimization,
Data):
aux_vA = vA
g0 = 9.810665
Re = 6371000
tb = trajectory.coast_time
for i in range(0, rocket.num_stages):
T = stage[i].thrust
ISP = stage[i].Isp
tb = tb + (stage[i].propellant_mass) / (T) * g0 * ISP
tb = Data[0][-1] + (stage[-1].propellant_mass) / (
stage[-1].thrust) * g0 * stage[-1].Isp
##############Ccntrol law for no coast ARC########################
#### Only for last stage
Obj = 1000
#Boundary Defined, for better convergence in Free flight
t_lb = -(tb - Data[0][-1]) * 0.5
t_ub = 0
aux3 = 0
m = GEKKO(remote=False)
m.time = np.linspace(0, 1, 100)
final = np.zeros(len(m.time))
final[-1] = 1
final = m.Param(value=final)
#Lagrange multipliers with l1=0
l2 = m.FV(-0.01, lb=trajectory.l2_lb, ub=trajectory.l2_ub)
l2.STATUS = 1
l3 = m.FV(-1, lb=trajectory.l3_lb, ub=trajectory.l3_ub)
l3.Status = 1
l4 = m.Var(value=0)
rate = m.Const(value=T / ISP / g0)
#value=(tb-Data[0][-1]+(t_ub+t_lb)/2)
tf = m.FV(value=(tb - Data[0][-1] + t_lb),
ub=tb - Data[0][-1] + t_ub,
lb=tb - Data[0][-1] + t_lb)
tf.STATUS = 1
aux = m.FV(0, lb=-5, ub=5)
aux.STATUS = 1
vD = m.FV(value=Data[8][-1])
#Initial Conditions
x = m.Var(value=Data[1][-1])
y = m.Var(value=Data[2][-1])
vx = m.Var(value=Data[3][-1] * cos(Data[4][-1]))
vy = m.Var(value=Data[3][-1] * sin(Data[4][-1]), lb=0)
vG = m.Var(value=Data[7][-1])
vA = m.Var(value=0)
gamma = m.Var()
alpha = m.Var()
t = m.Var(value=0)
m.Equations([l4.dt() / tf == -l2])
m.Equation(t.dt() / tf == 1)
mrt = m.Intermediate(Data[5][-1] - rate * t)
srt = m.Intermediate(m.sqrt(l3**2 + (l4 - aux)**2))
m.Equations([
x.dt() / tf == vx,
y.dt() / tf == vy,
vx.dt() / tf == (T / mrt * (-l3) / srt - (vx**2 + vy**2) /
(Re + y) * m.sin(gamma)),
vy.dt() / tf == (T / mrt * (-(l4 - aux) / srt) + (vx**2 + vy**2) /
(Re + y) * m.cos(gamma) - g0 * (Re / (Re + y))**2),
vG.dt() / tf == g0 * (Re / (Re + y))**2 * m.sin(m.atan(vy / vx))
])
m.Equation(gamma == m.atan(vy / vx))
m.Equation(alpha == m.atan((l4 - aux) / l3))
m.Equation(vA.dt() / tf == T / mrt - T / mrt * m.cos((alpha - gamma)))
#m.Obj(final*vA)
# Soft constraints
m.Obj(final * (y - mission.final_altitude)**2)
m.Obj(final * 100 *
(vx - mission.final_velocity * cos(mission.final_flight_angle))**2)
m.Obj(final * 100 *
(vy - mission.final_velocity * sin(mission.final_flight_angle))**2)
#m.Obj(100*tf)
m.Obj(final *
(l2 * vy + l3 *
(T / (Data[5][-1] - rate * t) * (-l3 / (m.sqrt(l3**2 +
(l4 - aux)**2))) -
(vx**2 + vy**2) / (Re + y) * m.sin(gamma)) + (l4 - aux) *
(T / (Data[5][-1] - rate * t) *
(-(l4 - aux) / (m.sqrt(l3**2 + (l4 - aux)**2))) + (vx**2 + vy**2) /
(Re + y) * m.cos(gamma) - g0 * (Re / (Re + y))**2) + 1)**2)
#Options
m.options.IMODE = 6
m.options.SOLVER = 1
m.options.NODES = 3
m.options.MAX_MEMORY = 10
m.options.MAX_ITER = 500
m.options.COLDSTART = 0
m.options.REDUCE = 0
m.solve(disp=False)
print("vA", vA.value[-1] + aux_vA)
print()
"""
#print("Obj= ",Obj)
print("altitude: ", y.value[-1], vx.value[-1], vy.value[-1])
print("Final mass=",Data[5][-1]-rate.value*t.value[-1])
print("Gravity= ",vG.value[-1], " Drag= ",vD.value[-1], " alpha= ", vA.value[-1]," Velocity= ", sqrt(vx.value[-1]**2+vy.value[-1]**2) )
print("Flight angle=",gamma.value[-1])
print("")
print("l2:",l2.value[-1],"l3:", l3.value[-1], "aux:", aux.value[-1])
print("tf:", tf.value[-1], "tb:",tb-Data[0][-1])
print("Obj:", Obj)
# if t_lb==-10.0 or t_ub==10.0:
# break
"""
tm = np.linspace(Data[0][-1], tf.value[-1] + Data[0][-1], 100)
mass = Data[5][-1]
Data[0].extend(tm[1:])
Data[1].extend(x.value[1:])
Data[2].extend(y.value[1:])
for i in range(1, 100):
Data[3].extend([sqrt(vx.value[i]**2 + vy.value[i]**2)])
Data[4].extend([atan(vy.value[i] / vx.value[i])])
Data[5].extend([mass - rate.value * t.value[i]])
Data[6].extend([atan((l4.value[i] - aux.value[i]) / l3.value[i])])
Data[7].extend(vG.value[1:])
Data[8].extend(vD.value[1:])
DV_required = mission.final_velocity + Data[7][-1] + Data[8][
-1] + aux_vA + vA.value[-1]
Obj = m.options.objfcnval + DV_required
return Data, DV_required, Obj
'''
#hot and cold temperature for m>1
Th_i1=m.Array(m.FV,(Nhe-1,Nh))
Tc_j1=m.Array(m.FV,(Nhe-1,Nc))
'''
#outlet temperature
Thin = m.Array(m.Var, (Nhe, Nh))
Tcin = m.Array(m.Var, (Nhe, Nc))
for mf in range(Nhe):
for i in range(Nh):
if mf == 0:
Thin[mf][i] = m.Param(value=trackhot[i])
else:
Thin[mf][i] = m.FV()
for j in range(Nc):
if mf == 0:
Tcin[mf][j] = m.Param(value=trackcold[j])
else:
Tcin[mf][j] = m.FV()
'''
Thin=np.concatenate((Th_i0,Th_i1),axis=0)
Tcin=np.concatenate((Tc_j1,Tc_j0),axis=0)
'''
whot = [[m.Var(value=0, lb=0, ub=1, integer=True) for mf in range(Nhe)]
for i in range(Nh)]
wcold = [[m.Var(value=0, lb=0, ub=1, integer=True) for mf in range(Nhe)]
for j in range(Nc)]
'''
# create GEKKO model
m = GEKKO(remote=False)
# time points
n = 501
m.time = np.linspace(0, 10, n)
# constants
E, c, r, k, U_max = 1, 17.5, 0.71, 80.5, 20
# fishing rate
u = m.MV(value=1, lb=0, ub=1)
u.STATUS = 1
u.DCOST = 0
x = m.Var(value=70) # fish population
# fish population balance
m.Equation(x.dt() == r * x * (1 - x / k) - u * U_max)
J = m.Var(value=0) # objective (profit)
Jf = m.FV() # final objective
Jf.STATUS = 1
m.Connection(Jf, J, pos2='end')
m.Equation(J.dt() == (E - c / x) * u * U_max)
m.Obj(-Jf) # maximize profit
m.options.IMODE = 6 # optimal control
m.options.NODES = 3 # collocation nodes
m.options.SOLVER = 3 # solver (IPOPT)
m.solve(disp=True) # Solve
print('Optimal Profit: ' + str(Jf.value[0]))
plt.figure(1) # plot results
plt.subplot(2, 1, 1)
plt.plot(m.time, J.value, 'r--', label='profit')
plt.plot(m.time, x.value, 'b-', label='fish')
plt.legend()
plt.subplot(2, 1, 2)
stop_sign = "no"
# set up the gekko model
m = GEKKO()
# set up the time
num_points = 50 # more points is more accurate, but every point adds 2 DOF
max_time = 500
m.time = np.linspace(0, 1, num_points)
# set up the Manipulated Variables
ac_ped = m.MV(value=0, lb=0, ub=100)
br_ped = m.MV(value=0, lb=0, ub=100)
# set up the time variable (to minimize)
tf = m.FV(value=100, lb=30, ub=max_time)
# turn them 'on'
for s in (ac_ped, br_ped, tf):
s.STATUS = 1
# set up the variables
x = m.Var(value=0, lb=0, ub=x_goal)
v = m.Var(value=0, lb=0, ub=speed_limit)
a = m.Var(value=0, ub=2, lb=-2)
# set up the gears (1 is in the gear, 0 is not in the gear)
in_gr_1 = m.MV(integer=True, value=1, lb=0, ub=1)
in_gr_2 = m.MV(integer=True, value=0, lb=0, ub=1)
in_gr_3 = m.MV(integer=True, value=0, lb=0, ub=1)
gear_ratio = m.Var(value=car.ge[0])
p.time = np.linspace(0, 0.5, 0.01) p.dl = p.MV() p.beta = p.Param(value=0.05) p.g = p.CV() p.t = p.Param(value=p.time) A = 20 K = 300 p.Equation(p.exp(-p.t * p.beta) * p.g.dt() == p.dl) p.options.IMODE = 4 #----- Moving Horizon Estimation Model Setup -----# mhe = GEKKO(remote=False) mhe.time = np.linspace(0, 0.5, 0.01) mhe.dl = mhe.MV() #output mhe.beta = mhe.FV(value=0.01, lb=0.0000001, ub=0.5) mhe.g = mhe.CV() #measured variable mhe.t = mhe.Param(value=mhe.time) mhe.Equation(mhe.dl == mhe.exp(-mhe.t * mhe.beta) * mhe.g.dt()) mhe.options.IMODE = 5 mhe.options.EV_TYPE = 1 mhe.options.DIAGLEVEL = 0 # STATUS = 0, optimizer doesn't adjust value # STATUS = 1, optimizer can adjust mhe.dl.STATUS = 0 mhe.beta.STATUS = 1 mhe.g.STATUS = 1 # FSTATUS = 0, no measurement
class MPCnode(Node):
def __init__(self, id, x, y, nrj, ctrlHrz, ctrlRes):
super().__init__(id, x, y, nrj)
self.verbose = True
self.ctrlHrz = ctrlHrz # Control Horizon
# time points
self.ctrlRes = ctrlRes # Control Resolution. Number of control steps within the control horizon
# constants
self.Egen = 1*10**-5
self.const = 0.6
self.packet = 1
self.E = 1
self.m = GEKKO(remote=False)
self.m.time = np.linspace( 0, self.ctrlHrz, self.ctrlRes)
#self.deltaDist = -1.5
self.deltaDist = self.m.FV(value = -1.5)
#self.deltaDist.STATUS = 1
# packet rate
self.pr = self.m.MV(value=self.PA, integer = True,lb=1,ub=20)
self.pr.STATUS = 1
self.pr.DCOST = 0
# Energy Stored
self.nrj = self.m.Var(value=0.05, lb=0) # Energy Amount
self.d = self.m.Var(value=70, lb = 0) # Distance to receiver
self.d2 = self.m.Intermediate(self.d**2)
# energy/pr balance
#self.m.Equation(self.d.dt() == -1.5)
self.m.Equation(self.d.dt() == self.deltaDist)
self.m.Equation(self.nrj >= self.Egen - ((Eelec+EDA)*self.packet + self.pr*self.pSize*(Eelec + Eamp * self.d2)))
self.m.Equation(self.nrj.dt() == self.Egen - ((Eelec+EDA)*self.packet + self.pr*self.pSize*(Eelec + Eamp * self.d2)))
# objective (profit)
self.J = self.m.Var(value=0)
# final objective
self.Jf = self.m.FV()
self.Jf.STATUS = 1
self.m.Connection(self.Jf,self.J,pos2='end')
self.m.Equation(self.J.dt() == self.pr*self.pSize)
# maximize profit
self.m.Obj(-self.Jf)
# options
self.m.options.IMODE = 6 # optimal control
self.m.options.NODES = 3 # collocation nodes
self.m.options.SOLVER = 1 # solver (IPOPT)
def resetGEKKO(self):
self.m = GEKKO(remote=False)
self.m.time = np.linspace( 0, self.ctrlHrz, self.ctrlRes)
self.pr = self.m.MV(value=self.PA, integer = True,lb=1,ub=20)
self.pr.STATUS = 1
self.pr.DCOST = 0
def getDeltaDist(self, sinkX, sinkY, sdeltaX, sdeltaY, deltaDist):
distBefore = np.sqrt((sinkX**2)+(sinkY**2))
distAfter = np.sqrt(((sinkX+sdeltaX)**2)+((sinkY+sdeltaY)**2))
self.deltaDist = distAfter - distBefore
return self.deltaDist
def controlPR(self, deltaD):
# solve optimization problem
self.deltaDist.value = 1.5
self.m.solve(disp=False)
self.setPR(self.pr.value[1])
#self.pr.value = self.pr.value[1]
#self.m.GUI()
# print profit
print('Optimal Profit: ' + str(self.Jf.value[0]))
if self.verbose:
# plot results
plt.figure(1)
plt.subplot(2,1,1)
plt.plot(self.m.time,self.J.value,'r--',label='packets')
plt.plot(self.m.time[-1],self.Jf.value[0],'ro',markersize=10,\
label='final packets = '+str(self.Jf.value[0]))
plt.plot(self.m.time,self.nrj.value,'b-',label='energy')
plt.ylabel('Value')
plt.legend()
plt.subplot(2,1,2)
plt.plot(self.m.time,self.pr.value,'k.-',label='packet rate')
plt.ylabel('Rate')
plt.xlabel('Time (yr)')
plt.legend()
plt.show()
fid = open(filename, 'a')
fid.write(str(i)+','+str(Q1d[i])+','+str(Q2d[i])+',' \
+str(a.T1)+','+str(a.T2)+'\n')
# close connection to Arduino
a.close()
fid.close()
except:
filename = 'https://apmonitor.com/pdc/uploads/Main/tclab_data2.txt'
# read either local file or use link if no TCLab
data = pd.read_csv(filename)
# Fit Parameters of Energy Balance
m = GEKKO() # Create GEKKO Model
# Parameters to Estimate
U = m.FV(value=10, lb=1, ub=20)
Us = m.FV(value=20, lb=5, ub=40)
alpha1 = m.FV(value=0.01, lb=0.001, ub=0.03) # W / % heater
alpha2 = m.FV(value=0.005, lb=0.001, ub=0.02) # W / % heater
tau = m.FV(value=10.0, lb=5.0, ub=60.0)
# Measured inputs
Q1 = m.Param()
Q2 = m.Param()
Ta = 21.0 + 273.15 # K
mass = 4.0 / 1000.0 # kg
Cp = 0.5 * 1000.0 # J/kg-K
A = 10.0 / 100.0**2 # Area not between heaters in m^2
As = 2.0 / 100.0**2 # Area between heaters in m^2
eps = 0.9 # Emissivity
#options
p.options.IMODE = 4
#p.u.FSTATUS = 1
#p.u.STATUS = 0
#%% Model
m = GEKKO()
m.time = np.linspace(0, 20,
41) #0-20 by 0.5 -- discretization must match simulation
#Parameters
m.u = m.MV() #input
m.K = m.FV(value=1, lb=1, ub=3) #gain
m.tau = m.FV(value=5, lb=1, ub=10) #time constant
#Variables
m.x = m.SV() #state variable
m.y = m.CV() #measurement
#Equations
m.Equations([m.tau * m.x.dt() == -m.x + m.u, m.y == m.K * m.x])
#Options
m.options.IMODE = 5 #MHE
m.options.EV_TYPE = 1
# STATUS = 0, optimizer doesn't adjust value
# STATUS = 1, optimizer can adjust
