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