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
connected = False # simulation m = GEKKO() m.time = np.linspace(0, n, n + 1) U = 5.0 A = 0.0012 Cp = 500 mass = 0.004 alpha = 0.01 Ta = 23 eps = 0.9 sigma = 5.67e-8 TaK = Ta + 273.15 TC = m.Var(Ta) TK = m.Intermediate(TC + 273.15) conv = m.Intermediate(U * A * (Ta - TC)) rad = m.Intermediate(sigma * eps * A * (TaK**4 - TK**4)) loss = m.Intermediate(conv + rad) gain = m.Intermediate(alpha * 50) m.Equation(mass * Cp * TC.dt() == conv + rad + gain) m.options.NODES = 3 m.options.IMODE = 4 # dynamic simulation m.solve(disp=False) # Plot results plt.figure() plt.subplot(2, 1, 1) plt.plot(m.time, TC, 'b-', label='Simulated') if connected: plt.plot(m.time, T1, 'r.', label='Measured')
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 sigma = 5.67e-8 # Stefan-Boltzmann TH1 = m.SV() TH2 = m.SV() TC1 = m.CV() TC2 = m.CV() # Heater Temperatures in Kelvin T1 = m.Intermediate(TH1 + 273.15) T2 = m.Intermediate(TH2 + 273.15) # Heat transfer between two heaters Q_C12 = m.Intermediate(Us * As * (T2 - T1)) # Convective Q_R12 = m.Intermediate(eps * sigma * As * (T2**4 - T1**4)) # Radiative # Energy balances m.Equation(TH1.dt() == (1.0/(mass*Cp))*(U*A*(Ta-T1) \ + eps * sigma * A * (Ta**4 - T1**4) \ + Q_C12 + Q_R12 \ + alpha1*Q1)) m.Equation(TH2.dt() == (1.0/(mass*Cp))*(U*A*(Ta-T2) \ + eps * sigma * A * (Ta**4 - T2**4) \ - Q_C12 - Q_R12 \
class MPCnode(Node): def __init__(self, id, x, y, nrj, ctrlHrz, ctrlRes): super().__init__(id, x, y, nrj) self.verbose = False self.errorFlag = False self.m = GEKKO(remote=False) # time points self.ctrlHrz = ctrlHrz # Control Horizon self.ctrlRes = ctrlRes # Control Resolution. Number of control steps within the control horizon self.m.time = np.linspace(0, self.ctrlHrz, self.ctrlRes) # constants self.Egen = 1 * 10**-5 #Counter for plots self.nrplots = 1 self.pltColors = ['r-', 'k-', 'b-', 'g-', 'y-', 'c-', 'm-'] self.labels = [ "Distance", "Velocity", "Energy Consumption", "Data Remaining", "Transmission Rate", "Battery" ] colorNr = rand.randrange(0, len(self.pltColors), 1) self.lineColor = self.pltColors[colorNr] #Threshold for when objective is seen as 0 self.limit = np.float64(1e-10) self.resetGEKKO() def resetGEKKO(self): self.errorFlag = False self.m = GEKKO(remote=False) self.m.time = np.linspace(0, self.ctrlHrz, self.ctrlRes) self.vp = np.zeros(self.ctrlRes) self.v = self.m.Param(value=self.vp) # define distance self.dist = self.m.Var() self.dtr = self.m.MV(value=0, integer=True, lb=0, ub=20) self.dtr.STATUS = 1 # define energy level self.nrj_stored = self.m.Var(value=self.energy, lb=0) self.data = self.m.Var() #self.data = self.m.CV() #self.data.STATUS = 1 #self.m.options.CV_TYPE = 2 # energy to transmit self.e = self.m.Intermediate(( (Eelec + EDA) * self.conChildren + self.dtr * self.pSize * (Eelec + Eamp * self.dist**2)) - self.Egen) #self.e = self.m.Var() #self.m.Equation(self.e == ((Eelec+EDA)*self.conChildren + self.dtr*self.pSize*(Eelec + Eamp * self.dist**2)) - self.Egen) # equations # track the position self.m.Equation(self.dist.dt() == self.v) self.m.Equation(self.nrj_stored.dt() == -self.e) self.m.Equation(self.nrj_stored >= self.e) # as data is transmitted, remaining data stored decreases self.m.Equation(self.data.dt() == -self.dtr * self.pSize) self.deadline = self.m.Var(value=self.ctrlHrz) self.dlCost = self.m.Var() self.m.Equation(self.deadline.dt() == -1) # self.m.Equation(self.energy >= self.e) # objective #self.data.SP = 0 # soft (objective constraint) self.recedingDeadline = np.zeros(self.ctrlRes) self.recedingDeadline[-1] = 100 self.final = self.m.Param(value=np.zeros(self.ctrlRes)) #self.final.value[self.ctrlRes//2:-1] = 0.001 self.final.value[-1] = 1 #self.m.Equation(self.final*(self.data)<=0) #self.m.Obj(self.e) # minimize energy #self.m.Obj(self.data*self.final) self.target = self.m.Intermediate((self.final * (0 - self.data)**2)) self.m.Obj(self.target) # transmit data by the end self.m.Obj(self.e) #self.m.Obj(self.final*self.data) # hard constraint # this form may cause infeasibility if it can't achieve # data=0 at the end #self.m.fix(self.data,self.ctrlRes-1,0) # options # Solutions forced to terminate early by the MAX_TIME # constraint do not satisfy the Karush Kuhn Tucker conditions for optimality. self.m.options.MAX_TIME = 10 self.m.options.IMODE = 6 # optimal control self.m.options.NODES = 3 # collocation nodes self.m.options.SOLVER = 1 # solver (IPOPT), 1 is for when integers is used as MV self.m.options.TIME_SHIFT = 1 # Setting for saving values from last solve() def produce_vVector(self, xVec, yVec): vVec = np.zeros(len(xVec)) for i in range(len(xVec) - 1): dist_Before = np.float64( np.sqrt((xVec[i] - self.xPos)**2 + (yVec[i] - self.yPos)**2)) dist_After = np.float64( np.sqrt((xVec[i + 1] - self.xPos)**2 + (yVec[i + 1] - self.yPos)**2)) vVec[i] = np.float64((dist_After - dist_Before) / (self.ctrlHrz / (self.ctrlRes - 1))) return vVec def setDesData(self, dat): if (type(self.data.value.value) is list): self.data.value[0] = dat * self.pSize else: self.data.value = dat * self.pSize def controlPR(self, velocity): tempVel = np.float64(velocity) tempNrj = np.float64(self.energy) tempDist = np.float64(self.getDistance(self.CHparent)) self.vp[0:] = self.v.value[1] if (tempVel == self.vp[0]): if self.verbose: print('Velocity: {0} was equal to vVal0: {1}'.format( tempVel, self.vp[0])) print('Therefore, velocity was set as vp[1:]') else: if self.verbose: print('Velocity: {0} was not equal to vVal0: {1}'.format( tempVel, self.v.value[0])) print('Therefore, vp[1:] was set as tempVel = {0}'.format( tempVel)) self.vp[1:] = tempVel self.v.value = self.vp if (type(self.data.value.value) is list): if (self.data.value.value[0] <= self.limit): self.datap = np.zeros(self.ctrlRes) self.data.value[0] = 0 #SETTING THE CURRENT ENERGY LEVEL if (type(self.nrj_stored.value.value) is list): self.nrj_stored.value[0] = tempNrj else: self.nrj_stored.value = tempNrj # define distance if (type(self.dist.value.value) is list): self.dist.value[0] = tempDist else: self.dist.value = tempDist if (type(self.dtr.value.value) is list): self.dtrp = np.zeros(self.ctrlRes) self.dtrp[0] = np.float64(self.PA) self.dtr.value = self.dtrp #self.dtr.value.value[0] = np.float64(self.PA) #self.nrj_stored.value[0] = self.nrj_stored.value[1] #self.data.value[0] = self.data.value[1] try: self.m.solve(disp=False) self.setPR(self.dtr.value[1]) #if (self.recedingDeadline[0] == 0): # self.recedingDeadline = np.roll(self.recedingDeadline,-1) #self.final.value = self.recedingDeadline except: print('EXCEPTION CAUGHT FOR NODE: {0}'.format(self.ID)) self.setPR(1) self.errorFlag = True def controlPR1(self, sink): self.v.value = self.produce_vVector(sink.xP.value, sink.yP.value) tempNrj = np.float64(self.energy) tempDist = np.float64(np.abs(self.getDistance(self.CHparent))) #if(type(self.data.value.value) is list): # if(self.data.value.value[0] <= self.limit): # self.data.value[0] = 0 #SETTING THE CURRENT ENERGY LEVEL if (type(self.nrj_stored.value.value) is list): #print('NRJ STORED WAS NOT FLOAT. IT WAS: {0}'.format(type(self.nrj_stored.value.value))) self.nrj_stored.value[0] = tempNrj else: self.nrj_stored.value = tempNrj # define distance if (type(self.dist.value.value) is list): #print('DIST WAS NOT INT, IT WAS: {0}'.format(type(self.dist.value.value))) self.dist.value[0] = tempDist else: self.dist.value = tempDist #if(self.data.value[0] <= limit): # self.data.value[0] = 0 #print(np.shape(testNode.vp)) if (type(self.dtr.value.value) is list): #self.dtrp = np.zeros(self.ctrlRes) #self.dtrp[0] = np.float64(self.PA) #self.dtr.value = self.dtrp self.dtr.value = self.dtr.NXTVAL #self.nrj_stored.value[0] = self.nrj_stored.value[1] #self.data.value[0] = self.data.value[1] """ self.dtrp = np.zeros(self.ctrlRes) self.dtrp[0] = self.dtr.value[1] self.dtr.value = self.dtrp self.nrj_storedp = np.zeros(self.ctrlRes) self.nrj_storedp[0] = self.nrj_stored.value[1] self.nrj_stored.value = self.nrj_storedp self.datap = np.zeros(self.ctrlRes) self.datap[0] = self.data.value[1] self.data.value = self.datap self.m.TIME_SHIFT = 1 """ #self.data.value[0] = self.data.value[1] try: self.m.solve(disp=False) self.setPR(self.dtr.value[1]) except: print('EXCEPTION CAUGHT') self.setPR(1) self.errorFlag = True def plot(self): #plt.cla() # Clear axis #plt.clf() # Clear figure #plt.close() # Close a figure window #by_label = OrderedDict(zip(labels, handles)) #plt.legend(by_label.values(), by_label.keys()) plt.figure(self.nrplots) plt.subplot(6, 1, 1) dist_line = plt.plot(self.m.time, self.dist.value, self.lineColor, label=self.labels[0]) distance_legend = plt.legend(handles=dist_line) plt.subplot(6, 1, 2) #plt.plot(self.m.time,self.v.value,self.lineColor,label=self.labels[1]) vel_line = plt.step(self.m.time, self.v.value, self.lineColor, label=self.labels[1], where='post') vel_legend = plt.legend(handles=vel_line) plt.subplot(6, 1, 3) #plt.plot(self.m.time,self.e.value,self.lineColor,label=self.labels[2]) energyCons_line = plt.step(self.m.time, self.e.value, self.lineColor, label=self.labels[2], where='post') nrjcons_legend = plt.legend(handles=energyCons_line) plt.subplot(6, 1, 4) dataRem_line = plt.plot(self.m.time, self.data.value, self.lineColor, label=self.labels[3]) #plt.bar(self.m.time, self.data.value, align='center', alpha=0.5) dataRem_legend = plt.legend(handles=dataRem_line) plt.subplot(6, 1, 5) #plt.plot(self.m.time, self.dtr.value,'r-',label='Transmission Rate') transmRate_line = plt.step(self.m.time, self.dtr.value, self.lineColor, label=self.labels[4], where='post') tr_legend = plt.legend(handles=transmRate_line) plt.subplot(6, 1, 6) battery_line = plt.plot(self.m.time, self.nrj_stored, self.lineColor, label=self.labels[5]) battery_legend = plt.legend(handles=battery_line) plt.xlabel('Time') #plt.show() self.nrplots += 1 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
#Paramters evap_c = m.Array(m.Param, 4, value=1e-5) evap_c[-1].value = 0.5e-5 A = [m.Param(value=i) for i in areas] #python list comprehension Vin[0] = m.Param(value=vin) #Variables V = [m.Var(value=i) for i in V0] h = [m.Var(value=i) for i in h0] Vout = [m.Var(value=i) for i in Vout0] #Intermediates Vin[1:4] = [m.Intermediate(Vout[i]) for i in range(3)] Vevap = [m.Intermediate(evap_c[i] * A[i]) for i in range(4)] #Equations m.Equations( [V[i].dt() == Vin[i] - Vout[i] - Vevap[i] - Vuse[i] for i in range(4)]) m.Equations([1000 * V[i] == h[i] * A[i] for i in range(4)]) m.Equations([Vout[i]**2 == c[i]**2 * h[i] for i in range(4)]) #Set to simulation mode m.options.imode = 4 #Solve m.solve() #%% Plot results
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
####################### # Model Decision Variables # Total Variables = 3 # Audit Coverage % x_1 = m.Var(value=NC_ATR_MODEL['Coverage Minimum'][AUDIT_NAMES[0]], lb=0, ub=1) x_2 = m.Var(value=NC_ATR_MODEL['Coverage Minimum'][AUDIT_NAMES[1]], lb=0, ub=1) x_3 = m.Var(value=NC_ATR_MODEL['Coverage Minimum'][AUDIT_NAMES[2]], lb=0, ub=1) ####################### # Intermediate Calculations # Compute Normalized Cumulative Additional Tax Revenue (NCATR) # for each audit class given audit coverage % (x) i_NCATR_1 = m.Intermediate(NC_ATR_MODEL['A'][AUDIT_NAMES[0]] * x_1**NC_ATR_MODEL['b'][AUDIT_NAMES[0]]) i_NCATR_2 = m.Intermediate(NC_ATR_MODEL['A'][AUDIT_NAMES[1]] * x_2**NC_ATR_MODEL['b'][AUDIT_NAMES[1]]) i_NCATR_3 = m.Intermediate(NC_ATR_MODEL['A'][AUDIT_NAMES[2]] * x_3**NC_ATR_MODEL['b'][AUDIT_NAMES[2]]) # Compute Cumulative Additional Tax Revenue (CATR) given the population i_CATR_1 = m.Intermediate(i_NCATR_1 * NC_ATR_MODEL['Population'][AUDIT_NAMES[0]]) i_CATR_2 = m.Intermediate(i_NCATR_2 * NC_ATR_MODEL['Population'][AUDIT_NAMES[1]]) i_CATR_3 = m.Intermediate(i_NCATR_3 * NC_ATR_MODEL['Population'][AUDIT_NAMES[2]]) # (Cost / Audit) * (Audit Coverage %) * Population = $ i_cost_1 = m.Intermediate(x_1 * NC_ATR_MODEL['Audit Cost'][AUDIT_NAMES[0]] *
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
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 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()
class gekko_model(): def __init__(self, x0, param0, physics0, x_target): x_pos = x0[0] y_pos = x0[1] x_vel = x0[2] y_vel = x0[3] heading = x0[4] ang_vel = x0[5] SPS_mass = x0[6] RCS_mass = x0[7] dry_mass = param0[0] SPS_thrust = param0[1] RCS_thrust = param0[2] SPS_mass_flow = param0[3] RCS_mass_flow = param0[4] J = param0[5] r = param0[6] x_pos_t = x_target[0] y_pos_t = x_target[1] x_vel_t = x_target[2] y_vel_t = x_target[3] mu = physics0[0] self.m = GEKKO(remote=False) self.x_t = self.m.Var(value=x_pos_t) self.y_t = self.m.Var(value=y_pos_t) self.x_vel_t = self.m.Var(value=x_vel_t) self.y_vel_t = self.m.Var(value=y_vel_t) self.dry_mass = self.m.Param(value=dry_mass) self.mu = self.m.Param(value=mu) self.SPS_flow = self.m.FV(value=SPS_mass_flow, lb=0, ub=SPS_mass) self.RCS_flow = self.m.FV(value=RCS_mass_flow, lb=0, ub=RCS_mass) self.r = self.m.Param(value=r) self.RCS_bias = self.m.FV(value=1.0, ub=1.5, lb=.5) self.SPS_bias = self.m.FV(value=1.0, ub=1.5, lb=.5) self.RCS_com = self.m.MV(value=0, lb=-1, ub=1, integer=True) self.SPS_com = self.m.MV(value=0, lb=0, ub=1, integer=True) self.x = self.m.CV(value=x_pos, name='x') self.y = self.m.CV(value=y_pos, name='y') self.x_vel = self.m.CV(value=x_vel, name='x_vel') self.y_vel = self.m.CV(value=y_vel, name='y_vel') self.heading = self.m.CV(value=heading, name='heading') self.ang_vel = self.m.CV(value=ang_vel, name='ang_vel') self.SPS_mass = self.m.Var(value=SPS_mass, lb=0, ub=SPS_mass, name='SPS_mass') self.RCS_mass = self.m.Var(value=RCS_mass, lb=0, ub=RCS_mass, name='RCS_mass') self.J = self.m.FV(value=J) self.SPS_thrust = self.m.FV(value=SPS_thrust) self.RCS_thrust = self.m.FV(value=RCS_thrust) self.SPS_thrust_f = self.m.Intermediate( self.SPS_com * self.SPS_thrust * self.SPS_bias) self.RCS_thrust_f = self.m.Intermediate( self.RCS_com * self.RCS_thrust * self.RCS_bias) self.d_t = self.m.Intermediate(self.m.sqrt(self.x_t**2 + self.y_t**2)) self.r_dd_t = self.m.Intermediate(-self.mu / self.d_t**3) self.mass_t = self.m.Intermediate(self.dry_mass + self.RCS_mass + self.SPS_mass) self.d = self.m.Intermediate(self.m.sqrt(self.x**2 + self.y**2)) self.r_dd = self.m.Intermediate(-self.mu / (self.d**3)) self.x_accel_g = self.m.Intermediate(self.x * self.r_dd) self.y_accel_g = self.m.Intermediate(self.y * self.r_dd) self.x_accel_SPS = self.m.Intermediate( self.SPS_thrust_f * self.m.sin(self.heading) / self.mass_t) self.y_accel_SPS = self.m.Intermediate( self.SPS_thrust_f * self.m.cos(self.heading) / self.mass_t) self.phase = self.m.Intermediate(self.m.atan(self.x / self.y)) self.phase_t = self.m.Intermediate(self.m.atan(self.x_t / self.y_t)) self.m.Equation(self.x.dt() == self.x_vel) self.m.Equation(self.y.dt() == self.y_vel) self.m.Equation(self.x_vel.dt() == self.x_accel_g + self.x_accel_SPS) self.m.Equation(self.y_vel.dt() == self.y_accel_g + self.y_accel_SPS) self.m.Equation(self.heading.dt() == self.ang_vel) self.m.Equation(self.ang_vel.dt() == self.RCS_thrust_f * self.r / self.J) self.m.Equation(self.SPS_mass.dt() == -self.SPS_flow * self.SPS_com * self.SPS_bias) self.m.Equation(self.RCS_mass.dt() == -self.RCS_flow * self.m.abs(self.RCS_com) * self.RCS_bias) self.m.Equation(self.x_t.dt() == self.x_vel_t) self.m.Equation(self.y_t.dt() == self.y_vel_t) self.m.Equation(self.x_vel_t.dt() == self.x_t * self.r_dd_t) self.m.Equation(self.y_vel_t.dt() == self.y_t * self.r_dd_t) def mpc_setup(self, time): self.m.time = time p = np.zeros(len(time)) p[-2:] = 1 p[:] = 1 self.final = self.m.Param(value=p) self.RCS_com.STATUS = 1 self.SPS_com.STATUS = 1 self.m.options.MAX_ITER = 1000 self.m.Equation( self.d >= 6471000 ) #Distance must be larger than the radius of the Earth + 100 km self.m.Equation(self.RCS_mass >= 0) #Must have positive mass in tanks self.m.Equation(self.SPS_mass >= 0) self.m.Equation(self.m.abs(self.ang_vel) <= 2 * 3.14) self.m.Obj((self.final * (self.d - self.d_t)**2)) self.m.Obj(1e12 * self.final * (self.phase - self.phase_t)**2) self.m.Obj((self.m.sqrt(self.x_vel**2 + self.y_vel**2) - self.m.sqrt(self.x_vel_t**2 + self.y_vel_t**2))**2) self.m.options.IMODE = 6 def mpc_update(self, x0, x_target): x_pos = x0[0] y_pos = x0[1] x_vel = x0[2] y_vel = x0[3] heading = x0[4] ang_vel = x0[5] SPS_mass = x0[6] RCS_mass = x0[7] x_pos_t = x_target[0] y_pos_t = x_target[1] x_vel_t = x_target[2] y_vel_t = x_target[3] self.x.VALUE = x_pos self.y.VALUE = y_pos self.x_vel.VALUE = x_vel self.y_vel.VALUE = y_vel self.heading.VALUE = heading self.ang_vel.VALUE = ang_vel self.SPS_mass.VALUE = SPS_mass self.RCS_mass.VALUE = RCS_mass self.x_t.VALUE = x_pos_t self.y_t.VALUE = y_pos_t self.x_vel_t.VALUE = x_vel_t self.y_vel_t.VALUE = y_vel_t def mpc_solve(self): try: self.m.solve(disp=False) print("Finished") RCS_com = self.RCS_com.NEWVAL SPS_com = self.SPS_com.NEWVAL except: print("Did not finish") RCS_com = 0 SPS_com = 0 return ([SPS_com, RCS_com]) def mhe_setup(self, time): self.m.time = time self.m.options.EV_TYPE = 1 self.m.options.IMODE = 5 self.m.options.ICD_CALC = 1 def mhe_on(self): self.x.FSTATUS = 1 #Receive measurements self.y.FSTATUS = 1 self.ang_vel.FSTATUS = 1 self.RCS_com.FSTATUS = 1 self.SPS_com.FSTATUS = 1 self.SPS_bias.FSTATUS = 0 self.RCS_bias.FSTATUS = 0 self.x.STATUS = 1 self.y.STATUS = 1 self.ang_vel.STATUS = 1 self.RCS_com.STATUS = 0 self.SPS_com.STATUS = 0 self.SPS_bias.STATUS = 1 self.RCS_bias.STATUS = 1 self.SPS_bias.DMAX = .05 self.RCS_bias.DMAX = .05 def mhe_add(self, x, y, ang_vel, SPS_com, RCS_com): self.x.MEAS = x self.y.MEAS = y self.ang_vel.MEAS = ang_vel self.SPS_com.MEAS = SPS_com self.RCS_com.MEAS = RCS_com def mhe_solve(self, x, y, ang_vel, SPS_com, RCS_com): self.x.MEAS = x self.y.MEAS = y self.ang_vel.MEAS = ang_vel self.SPS_com.MEAS = SPS_com self.RCS_com.MEAS = RCS_com self.m.solve(disp=False) return ([self.SPS_bias.NEWVAL, self.RCS_bias.NEWVAL], [self.x.MODEL, self.y.MODEL, self.ang_vel.MODEL]) def sim_setup(self, time): self.m.time = time self.RCS_com.FSTATUS = 1 self.SPS_com.FSTATUS = 1 self.m.options.SOLVER = 3 self.m.options.IMODE = 4 def sim_solve(self, SPS_com, RCS_com): self.RCS_com.MEAS = RCS_com self.SPS_com.MEAS = SPS_com self.SPS_com.STATUS = 0 self.RCS_com.STATUS = 0 self.m.solve(disp=False) return ([self.x.MODEL, self.y.MODEL, self.ang_vel.MODEL])
def reservoirs(): #Initial conditions c = np.array([0.03, 0.015, 0.06, 0]) areas = np.array([13.4, 12, 384.5, 4400]) V0 = np.array([0.26, 0.18, 0.68, 22]) h0 = 1000 * V0 / areas Vout0 = c * np.sqrt(h0) vin = [ 0.13, 0.13, 0.13, 0.21, 0.21, 0.21, 0.13, 0.13, 0.13, 0.13, 0.13, 0.13, 0.13 ] Vin = [0, 0, 0, 0] #Initialize model m = GEKKO() #time array m.time = np.linspace(0, 1, 13) #define constants #ThunderSnow Constants exist c = m.Array(m.Const, 4, value=0) c[0].value = 0.03 c[1].value = c[0] / 2 c[2].value = c[0] * 2 c[3].value = 0 #python constants are equivilant to ThunderSnow constants Vuse = [0.03, 0.05, 0.02, 0.00] #Paramters evap_c = m.Array(m.Param, 4, value=1e-5) evap_c[-1].value = 0.5e-5 A = [m.Param(value=i) for i in areas] #python list comprehension Vin[0] = m.Param(value=vin) #Variables V = [m.Var(value=i) for i in V0] h = [m.Var(value=i) for i in h0] Vout = [m.Var(value=i) for i in Vout0] #Intermediates Vin[1:4] = [m.Intermediate(Vout[i]) for i in range(3)] Vevap = [m.Intermediate(evap_c[i] * A[i]) for i in range(4)] #Equations m.Equations( [V[i].dt() == Vin[i] - Vout[i] - Vevap[i] - Vuse[i] for i in range(4)]) m.Equations([1000 * V[i] == h[i] * A[i] for i in range(4)]) m.Equations([Vout[i]**2 == c[i]**2 * h[i] for i in range(4)]) #Set to simulation mode m.options.imode = 4 #Solve m.solve(disp=False) hvals = [h[i].value for i in range(4)] assert (test.like( hvals, [[ 19.40299, 19.20641, 19.01394, 19.31262, 19.60511, 19.89159, 19.6849, 19.48247, 19.28424, 19.09014, 18.90011, 18.71408, 18.53199 ], [ 15.0, 15.15939, 15.31216, 15.46995, 15.63249, 15.79955, 15.95968, 16.11305, 16.25983, 16.40018, 16.53428, 16.66226, 16.7843 ], [ 1.768531, 1.758775, 1.74913, 1.739597, 1.730178, 1.720873, 1.71168, 1.702594, 1.693612, 1.684731, 1.675947, 1.667259, 1.658662 ], [ 5.0, 5.001073, 5.002143, 5.003208, 5.004269, 5.005326, 5.00638, 5.007429, 5.008475, 5.009516, 5.010554, 5.011589, 5.012619 ]]))
name='Cp_air') #J/kgK (found using HYSYS calculations) kf_air = m.Const(value=.01855, name='kf_air') #W/mK (found using HYSYS calculations) mass_batt = m.Const(value=142 / 4, name='mass_batt') #kg Cp_batt = m.Const(value=900, name='Cp_batt') #J/kgK kf_batt = m.Const(value=6, name='kf_batt') #W/mK radius_batt = m.Const(value=.5 / 2, name='radius_batt') #m (Estimation based on photographs) length_total = m.Const( value=2, name='length_total') #m (Estimation based on photographs) length_motor = m.Const(value=.5, name='length_motor') #m (Guess) V_open_circuit = m.Const(value=44, name='V_open_circuit') #Volts mu2 = m.Param(value=mu, name='mu2') #Intermediates length_batt = m.Intermediate(length_total - length_motor, name='length_batt') #m Pr = m.Intermediate(Cp_air * mu2 / kf_air, name='Pr') ################ Variables only discretized against time ###################### #Constants and values for convective heat transfer motor_power = m.Param(value=motor, name='motor_power') #W (Not used) t_air2 = m.Param(value=t_air, name='t_air2') #K T_SP2 = m.Param(value=T_SP, name='T_SP2') #K re = m.Param(value=re2, name='re') eff_motor = m.Param(value=motor_efficiency, name='eff_motor') I_load = m.Param(value=motor_current, name='I_load') #Amps V_load = m.Param(value=motor_voltage, name='V_load') #Volts R_batt = m.Const( value=.02,
class MPCnode(Node): def __init__(self, id, x, y, nrj, ctrlHrz, ctrlRes): super().__init__(id, x, y, nrj) self.verbose = False self.errorFlag = False # Creates the optimizing GEKKO module #self.m = GEKKO(remote=False) # Horizon, time points self.ctrlHrz = ctrlHrz # Control Horizon self.ctrlRes = ctrlRes # Control Resolution. Number of control steps within the control horizon #self.m.time = np.linspace( 0, self.ctrlHrz, self.ctrlRes) self.desData = 0 self.DLcounter = 1 # Counts control steps during a round in order to be able to move the deadline forward after each control cycle # constants self.Egen = 1 * 10**-5 self.Egen = 0 # Counter for plots, list for plot colors to choose from self.nrplots = 1 self.pltColors = ['r-', 'k-', 'b-', 'g-', 'y-', 'c-', 'm-'] self.labels = [ "Distance", "Velocity", "Energy Consumption", "Data Remaining", "Transmission Rate", "Battery" ] colorNr = rand.randrange(0, len(self.pltColors), 1) self.lineColor = self.pltColors[colorNr] # Resets the gekko module so that the optimizing can be done with new parameters and variables #self.resetGEKKO() def resetGEKKO(self): self.errorFlag = False # Sets errorflag back to false self.m = GEKKO(remote=False) # Creates new GEKKO module self.m.time = np.linspace(0, self.ctrlHrz, self.ctrlRes) self.vp = np.zeros(self.ctrlRes) self.v = self.m.Param(value=self.vp) # define distance self.dist = self.m.Var() self.dtr = self.m.MV(value=0, integer=True, lb=0, ub=20) self.dtr.STATUS = 1 # define energy level self.nrj_stored = self.m.Var(value=self.energy, lb=0) self.data = self.m.Var(value=self.desData) self.DSround = 0 # Data sent this round self.e = self.m.Intermediate(( (Eelec + EDA) * self.conChildren + self.dtr * self.pSize * (Eelec + Eamp * self.dist**2)) - self.Egen) # equations # track the position self.m.Equation(self.dist.dt() == self.v) self.m.Equation(self.nrj_stored.dt() == -self.e) self.m.Equation(self.nrj_stored >= self.e) # as data is transmitted, remaining data stored decreases self.m.Equation(self.data.dt() == -self.dtr * self.pSize) self.deadline = self.m.Var(value=self.ctrlHrz) self.dlCost = self.m.Var() self.m.Equation(self.deadline.dt() == -1) # self.m.Equation(self.energy >= self.e) # objective #self.data.SP = 0 # soft (objective constraint) self.final = self.m.Param(value=np.zeros(self.ctrlRes)) self.final.value[-1] = 1 self.target = self.m.Intermediate((self.final * (0 - self.data)**2)) self.m.Obj(self.target) # transmit data by the end self.m.Obj(self.e) #self.m.Obj(self.final*self.data) # options # Solutions forced to terminate early by the MAX_TIME constraint do # not satisfy the Karush Kuhn Tucker conditions for optimality. self.m.options.MAX_TIME = 10 self.m.options.IMODE = 6 # optimal control self.m.options.NODES = 3 # collocation nodes #self.m.options.CTRLMODE = 3 self.m.options.SOLVER = 1 # solver (IPOPT), 1 is for when integers is used as MV self.m.options.TIME_SHIFT = 1 # Setting for saving values from last solve() def produce_vVector(self, xVec, yVec): vVec = np.zeros(len(xVec)) for i in range(len(xVec) - 1): dist_Before = np.float64( np.sqrt((xVec[i] - self.xPos)**2 + (yVec[i] - self.yPos)**2)) dist_After = np.float64( np.sqrt((xVec[i + 1] - self.xPos)**2 + (yVec[i + 1] - self.yPos)**2)) vVec[i] = np.float64((dist_After - dist_Before) / (self.ctrlHrz / (self.ctrlRes - 1))) return vVec def setDesData(self, dat): self.desData = dat * self.pSize #if(type(self.data.value.value) is list): # self.data.value[0] = dat*self.pSize #else: # self.data.value = dat*self.pSize def plot(self, **kwargs): plt.figure(self.nrplots) plt.subplot(6, 1, 1) dist_line = plt.plot(self.m.time[1:], self.dist.value[1:], self.lineColor, label=self.labels[0]) distance_legend = plt.legend(handles=dist_line) plt.subplot(6, 1, 2) #plt.plot(self.m.time,self.v.value,self.lineColor,label=self.labels[1]) vel_line = plt.step(self.m.time[1:], self.v.value[1:], self.lineColor, label=self.labels[1], where='post') vel_legend = plt.legend(handles=vel_line) plt.subplot(6, 1, 3) #plt.plot(self.m.time,self.e.value,self.lineColor,label=self.labels[2]) energyCons_line = plt.step(self.m.time[1:], self.e.value[1:], self.lineColor, label=self.labels[2], where='post') nrjcons_legend = plt.legend(handles=energyCons_line) plt.subplot(6, 1, 4) dataRem_line = plt.plot(self.m.time[1:], self.data.value[1:], self.lineColor, label=self.labels[3]) #plt.bar(self.m.time, self.data.value, align='center', alpha=0.5) dataRem_legend = plt.legend(handles=dataRem_line) plt.subplot(6, 1, 5) #plt.plot(self.m.time, self.dtr.value,'r-',label='Transmission Rate') transmRate_line = plt.step(self.m.time[1:], self.dtr.value[1:], self.lineColor, label=self.labels[4], where='post') tr_legend = plt.legend(handles=transmRate_line) plt.subplot(6, 1, 6) battery_line = plt.plot(self.m.time[1:], self.nrj_stored[1:], self.lineColor, label=self.labels[5]) battery_legend = plt.legend(handles=battery_line) if ('timesegment' in kwargs): plt.xlabel('Time, Segm {0}'.format(kwargs['timesegment'])) else: plt.xlabel('Time') #plt.show() self.nrplots += 1 def controlPR(self, sink): #print('ELELELELE') #print(self.desData) #print(self.data.value) self.resetGEKKO() #self.data.value = self.desData #print('ELELELELE SECOND TIME') #print(self.desData) #print(self.data.value) self.v.value = self.produce_vVector(sink.xP.value, sink.yP.value) tempNrj = np.float64(self.energy) tempDist = np.float64(np.abs(self.getDistance(self.CHparent))) #SETTING THE CURRENT ENERGY LEVEL if (type(self.nrj_stored.value.value) is list): self.nrj_stored.value[0] = tempNrj else: self.nrj_stored.value = tempNrj # define distance if (type(self.dist.value.value) is list): self.dist.value[0] = tempDist else: self.dist.value = tempDist #if(type(self.dtr.value.value) is list): # print('dtr.value BEFORE: {0}'.format(self.dtr.value)) # self.dtr.value = self.dtr.value[1] # print('dtr.value AFTER: {0}'.format(self.dtr.value)) """ In order to move the horizon forward to make sure that the deadline is held (finish transmitting it's desired data before the time segments run out), the following if-block checks if the difference between total time segments and past time segments is equal or smaller than the controller's horizon. If it is, it is time to start rolling the end-value point forward. The objective function prioritizes solutions that reaches the desired state in steps before final[wherever the 1-value is]. """ if ((time_segments - self.DLcounter) <= self.ctrlHrz): self.final.value = np.roll( self.final.value, -((self.ctrlHrz) - (time_segments - self.DLcounter))) try: self.m.solve(disp=False) self.PA = self.dtr.value[1] self.desData -= self.dtr.value.value[1] * self.pSize self.DLcounter += 1 except: print('EXCEPTION CAUGHT FOR NODE {0}'.format(self.ID)) self.setPR(0) self.errorFlag = True self.DLcounter += 1 if (self.DLcounter > time_segments): self.DLcounter = 1 rmtree(self.m._path)
class MPCsink(Sink): def __init__(self, sizex, sizey, ctrlHrz, ctrlRes): super().__init__(sizex, sizey) self.ctrlHrz = ctrlHrz self.ctrlRes = ctrlRes self.v = 1 #Counter for plots self.nrplots = 1 self.xTarg = xSize / 2 self.yTarg = ySize / 2 #self.resetGEKKO() def resetGEKKO(self): self.m = GEKKO(remote=False) self.m.time = np.linspace(0, self.ctrlHrz, self.ctrlRes) self.m.TIME_SHIFT = 1 self.xMove = self.m.MV(integer=True, lb=-self.v, ub=self.v) self.xMove.STATUS = 1 self.yMove = self.m.MV(integer=True, lb=-self.v, ub=self.v) self.yMove.STATUS = 1 self.xP = self.m.Var(value=self.xPos, lb=0, ub=xSize) self.yP = self.m.Var(value=self.yPos, lb=0, ub=ySize) self.xTar = self.m.Param(value=self.xTarg) self.yTar = self.m.Param(value=self.yTarg) self.xDist = self.m.CV() self.xDist.STATUS = 1 self.yDist = self.m.CV() self.yDist.STATUS = 1 self.m.options.CV_TYPE = 2 #Squared Error self.xDist.value = self.xP.value - self.xTarg self.yDist.value = self.yP.value - self.yTarg self.m.Equation(self.xDist == self.xP - self.xTar) self.m.Equation(self.yDist == self.yP - self.yTar) self.m.Equation(self.xDist.dt() == self.xMove) self.m.Equation(self.yDist.dt() == self.yMove) self.xErr = self.m.Intermediate((self.xTar - self.xP)) self.yErr = self.m.Intermediate((self.yTar - self.yP)) self.xDist.SP = 0 self.yDist.SP = 0 self.m.options.IMODE = 6 self.m.options.MAX_TIME = 10 def setTarPoint(self, x, y): self.xTarg = x self.yTarg = y def produce_MoveVector(self): self.resetGEKKO() """ if(type(self.xDist.value.value) is not list): self.xDist.value = self.xP.value - self.xTar.value self.yDist.value = self.yP.value - self.yTar.value else: self.xMove[0] = self.xMove.NXTVAL self.yMove[0] = self.yMove.NXTVAL self.xP[0] = self.xPos self.yP[0] = self.yPos if(type(self.xTar.value.value) is list): self.xTar.value = self.xTar.value.value[0] self.yTar.value = self.yTar.value.value[0] self.xDist.value = self.xP.value[0] - self.xTar.value.value self.yDist.value = self.yP.value[0] - self.yTar.value.value else: self.xDist.value[0] = self.xP.value[0] - self.xTar.value.value self.yDist.value[0] = self.yP.value[0] - self.yTar.value.value """ #print('xTar: {0}\n yTar: {1}\n xDst: {2}\n yDst: {3}\n'.format(self.xTar.value,self.yTar.value,self.xDist.value,self.yDist.value)) self.m.solve(disp=False) #print('SECOND PRINT') #print('xTar: {0}\n yTar: {1}\n xDst: {2}\n yDst: {3}\n'.format(self.xTar.value,self.yTar.value,self.xDist.value,self.yDist.value)) rmtree(self.m._path) def plot(self): plt.figure(self.nrplots) plt.subplot(4, 1, 1) plt.step(self.m.time, self.xDist, 'r-', label='X-DIST') plt.legend() plt.subplot(4, 1, 2) plt.step(self.m.time, self.xMove, 'b-', label='X-VEL') plt.legend() plt.subplot(4, 1, 3) #plt.plot(self.m.time,self.v.value,'k--',label='Velocity') plt.step(self.m.time, self.yDist, 'k--', label='Y-DIST') plt.legend() plt.subplot(4, 1, 4) plt.step(self.m.time, self.yMove, 'b-', label='Y-VEL') plt.legend() self.nrplots += 1
import matplotlib.pyplot as plt #%% Process p = GEKKO() p.time = [0, .5] n = 1 #process model order #Parameters p.u = p.MV() p.K = p.Param(value=1) #gain p.tau = p.Param(value=5) #time constant #Intermediate p.x = [p.Intermediate(p.u)] #Variables p.x.extend([p.Var() for _ in range(n)]) #state variables p.y = p.SV() #measurement #Equations p.Equations( [p.tau / n * p.x[i + 1].dt() == -p.x[i + 1] + p.x[i] for i in range(n)]) p.Equation(p.y == p.K * p.x[n]) #options p.options.IMODE = 4 #p.u.FSTATUS = 1 #p.u.STATUS = 0
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)
m = GEKKO() t = time.time() s = sum(v) y = m.Var() m.Equation(y == s) m.solve(disp=False) print(y.value[0]) print('Elapsed time: ' + str(time.time() - t)) m.cleanup() # summation method 2 - Intermediates m = GEKKO() t = time.time() s = 0 for i in range(n): s = m.Intermediate(s + v[i]) y = m.Var() m.Equation(y == s) m.solve(disp=False) print(y.value[0]) print('Elapsed time: ' + str(time.time() - t)) m.cleanup() # summation method 3 - Gekko sum m = GEKKO() t = time.time() s = m.sum(v) y = m.Var() m.Equation(y == s) m.solve(disp=False) print(y.value[0])
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) # 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))) # forward reaction r1 = reaction_model.Intermediate(k1 * Ph_2_minus**alpha) # backwards reaction 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 E_a_1.STATUS = 1 E_a_2.STATUS = 1 A_1.STATUS = 0
# constants c1 = 0.13 c2 = 0.20 Ac = 2 # m^2 # inflow of volume per time qin1 = 0.5 # m^3/hr # variables h1 = m.Var(value=0, lb=0, ub=1) h2 = m.Var(value=0, lb=0, ub=1) overflow1 = m.Var(value=0, lb=0) overflow2 = m.Var(value=0, lb=0) # outflow equations qin2 = m.Intermediate(c1 * h1**0.5) qout1 = m.Intermediate(qin2 + overflow1) qout2 = m.Intermediate(c2 * h2**0.5 + overflow2) # mass balance equations m.Equation(Ac * h1.dt() == qin1 - qout1) m.Equation(Ac * h2.dt() == qin2 - qout2) # Minimize overflow. m.Obj(overflow1 + overflow2) # Set options. m.options.IMODE = 6 # dynamic optimization # Simulate differential equations. m.solve()
R = m.Var(value=0.0) # Parameters l_e = m.Param(value=50.0) R0 = m.Param(value=17.0) p = m.Param(value=1.0) tau = m.Param(value=0.7) theta = m.Param(value=0.5) nu = m.Param(value=0.5) eta = m.Param(value=0.5) tInf = m.Param(value=21.0) tImm = m.Param(value=20.0) tImm_V = m.Param(value=50.0) # Intermediates mu = m.Intermediate(1 / l_e) beta = m.Intermediate(R0 * (gamma + mu)) gamma = m.Intermediate(365 / tInf) sigma = m.Intermediate(1 / tImm) sigmaV = m.Intermediate(1 / tImm_V) strain1_frac = m.Intermediate((I1 + J1) / N) strain2_frac = m.Intermediate((I2 + J2 + Iv2) / N) S_frac = m.Intermediate(S / N) V_frac = m.Intermediate(V / N) R_1_frac = m.Intermediate((R1 + R) / N) R_2_frac = m.Intermediate((R2 + R) / N) R_frac = m.Intermediate(R / N) r1 = m.Intermediate(mu * (1 - p) * N) r2 = m.Intermediate(mu * p * N) r3 = m.Intermediate(mu * S) r4 = m.Intermediate(mu * V)
from gekko import GEKKO import numpy as np m = GEKKO(remote=False) x1 = m.Param(-2) x2 = m.Param(-1) x3 = np.linspace(0, 1, 6) x4 = m.Array(m.Param, 3) y4 = m.Array(m.Var, 3) y5 = m.Intermediate(3) for i in range(3): x4[i].value = -0.2 y4[i] = m.abs3(x4[i]) # create variable y = m.Var() # y = 3.6 = -2 -1 + 3 + 0 +3 + 0.6 m.Equation(y == m.sum([x1, x2]) + m.sum(x3) + m.sum([x1 + x2, x3, y5]) + sum(y4)) m.solve() # solve print('x1: ' + str(x1.value)) print('x2: ' + str(x2.value)) print('y: ' + str(y.value))
import matplotlib.pyplot as plt m = GEKKO() nt = 101 m.time = np.linspace(0, 1, nt) x1 = m.Var(value=1) x2 = m.Var(value=0) T = m.Var(value=380, lb=298, ub=398) final = np.zeros(nt) final[-1] = 1 final = m.Param(value=final) k1 = m.Intermediate(4000 * m.exp(-2500 / T)) k2 = m.Intermediate(6.2e5 * m.exp(-5000 / T)) m.Equation(x1.dt() == -k1 * x1**2) m.Equation(x2.dt() == k1 * x1**2 - k2 * x2) m.Obj(final * x2) m.options.IMODE = 6 m.solve(remote=False) plt.figure() plt.subplot(2, 1, 1) plt.plot(m.time, x1.value) plt.plot(m.time, x2.value)
# one-sided bounds #alpha.LOWER = 0 #beta.LOWER = 0 # state Variables Ph_3_minus = reaction_model.SV(Ph_3_minus_initial) Ph_3_minus_2 = reaction_model.SV(Ph_3_minus_initial_2) Ph_3_minus_3 = reaction_model.SV(Ph_3_minus_initial_3) # variable we will use to regress other Parameters Ph_2_minus = reaction_model.CV(ph_abs) Ph_2_minus_2 = reaction_model.CV(ph_abs_2) Ph_2_minus_3 = reaction_model.CV(ph_abs_3) # 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))) k3 = reaction_model.Intermediate(A_1 * reaction_model.exp(-E_a_1 / (R * T_2))) k4 = reaction_model.Intermediate(A_2 * reaction_model.exp(-E_a_2 / (R * T_2))) k5 = reaction_model.Intermediate(A_1 * reaction_model.exp(-E_a_1 / (R * T_3))) k6 = reaction_model.Intermediate(A_2 * reaction_model.exp(-E_a_2 / (R * T_3))) # forward reaction r1 = reaction_model.Intermediate(k1 * Ph_2_minus**alpha) # backwards reaction r2 = reaction_model.Intermediate(k2 * Ph_3_minus**beta) # forward reaction r3 = reaction_model.Intermediate(k3 * Ph_2_minus_2**alpha) # backwards reaction r4 = reaction_model.Intermediate(k4 * Ph_3_minus_2**beta) # forward reaction
class MPC2ndLayer(): def __init__(self): self.verbose = False self.nrplots = 0 self.m = GEKKO(remote=False) # constants self.Egen = 1 * 10**-5 self.pSize = ps self.PERC = 0.4 self.nds = 50 # ALL NODES self.CHs = int(self.nds * self.PERC) # CLUSTER HEADS self.nonCHs = int(self.nds - self.CHs) # NON-CHs self.CHLst = [] # List of CH positions Param() self.CHdistLst = [] # List of CHs distances Var() self.nonCHLst = [] # List of non-CH positions Param() self.nonCHdistLst = [] # List of non-CHs distances Var() self.intermeds = [ ] # List of intermediate equations for energy consumption of nodes #Counter for plots self.nrplots = 1 """ This part is in case the LEACH protocol is to be used in the model. self.dm1 = np.sum(self.CHdistLst)/len(self.CHdistLst) # Mean distance of CH to sink self.dm2 = np.sum(self.nonCHdistLst)/len(self.nonCHdistLst) # Mean distance of nonCH to sink self.e1 = self.m.Intermediate(((Eelec+EDA)*self.packet + self.packs*self.pSize*(Eelec + Eamp * self.dm1**2))*len(self.CHdistLst)) # Mean energy consumption per round for CHs self.e2 = self.m.Intermediate((self.packs*self.pSize*(Eelec + Eamp * self.dm2**2))*len(self.nonCHdistLst)) # Mean energy consumption per round for CHs self.rnd = self.m.Intermediate(self.E_tot/(self.PERC*self.E_tot*self.e1+(1-self.PERC)*self.e2)) """ # options self.m.options.IMODE = 3 # optimize a solid state def controlEnv(self): #WORKING SCRIPT FOR ONE DIMENSION self.sinkPos = self.m.Var( lb=0, ub=100 ) # Upper bound should be something like sqrt(100**2 + 100**2) #self.sinkV = self.m.MV(lb=-3,ub=3) #self.sinkV.STATUS = 1 # as data is transmitted, remaining data stored decreases for i in range(self.CHs): self.CHLst.append(self.m.Param(value=4 * i)) self.CHdistLst.append( self.m.Var(value=self.sinkPos.value - self.CHLst[i].value)) self.m.Equation(self.CHdistLst[i] == self.sinkPos - self.CHLst[i]) for i in range(self.nonCHs): self.nonCHLst.append(self.m.Param(value=2 * i)) self.nonCHdistLst.append( self.m.Var(value=self.sinkPos.value - self.nonCHLst[i].value)) self.m.Equation(self.nonCHdistLst[i] == self.sinkPos - self.nonCHLst[i]) self.packs = 1 self.dtrLst = [] self.rnds = self.m.Var(integer=True, lb=1) self.E_tot = self.m.Param(value=0.010) self.e1Sum = [] self.e1Vars = [] self.e2Sum = [] self.e2Vars = [] for i in range(self.CHs): self.dtrLst.append(self.m.Var(lb=1, ub=20)) self.e1Sum.append( self.m.Intermediate((Eelec + EDA) * self.packs + self.dtrLst[-1] * self.pSize * (Eelec + Eamp * self.CHdistLst[i]**2))) self.e1Vars.append(self.m.Var(lb=0)) self.m.Equation(self.e1Vars[-1] == (Eelec + EDA) * self.packs + self.dtrLst[-1] * self.pSize * (Eelec + Eamp * self.CHdistLst[i]**2)) for i in range(self.nonCHs): self.e2Sum.append( self.m.Intermediate(self.packs * self.pSize * (Eelec + Eamp * self.nonCHdistLst[i]**2))) self.e2Vars.append(self.m.Var(lb=0)) self.m.Equation( self.e2Vars[-1] == (self.packs * self.pSize * (Eelec + Eamp * self.nonCHdistLst[i]**2))) #self.m.Equation(self.E_tot >= (self.m.sum(self.e1Sum)+ self.m.sum(self.e2Sum))*self.rnds) self.m.Equation( self.E_tot >= (self.m.sum(self.e1Vars) + self.m.sum(self.e2Vars)) * self.rnds) self.target = self.m.Intermediate(self.m.sum(self.dtrLst)) self.m.Obj(-self.target * self.rnds) self.m.solve(disp=False) print('Sink\'s optimal Position: {0}'.format(self.sinkPos.value)) print('Number of rounds for optimal amount of data transmitted: {0}'. format(self.rnds.value)) def plot(self): #WORKING CODE objects = np.linspace(1, len(self.CHLst), len(self.CHLst)) #for i in range(self.CHLst): # objects.append(self.CHdistLst[i].) y_pos = np.arange(len(objects)) ydtr = np.linspace(0, 20, 11) ydist = np.linspace(-50, 50, 11) CHDL = [] for i in range(len(self.CHdistLst)): CHDL.append(self.CHdistLst[i][0]) DTRL = [] for i in range(len(self.dtrLst)): DTRL.append(self.dtrLst[i][0]) plt.figure(self.nrplots) plt.subplot(2, 1, 1) plt.bar(y_pos, CHDL, align='center', alpha=0.5) plt.yticks(ydist, ydist) plt.ylabel('distance') plt.subplot(2, 1, 2) plt.bar(y_pos, DTRL, align='center', alpha=0.5) plt.yticks(ydtr, ydtr) plt.ylabel('Packets Desired') plt.xlabel('Node#') plt.show() def clearGEKKO(self): self.m.clear()
#time m.time = np.linspace(0, 20, 41) #constants mass = 500 #Parameters b = m.Param(value=50) K = m.Param(value=0.8) #Manipulated variable p = m.MV(value=0, lb=0, ub=100) #Controlled Variable v = m.CV(value=0) i = m.Intermediate(-v * b) #Equations m.Equation(mass * v.dt() == i + K * b * p) #%% Tuning #global m.options.IMODE = 6 #control #MV tuning p.STATUS = 1 #allow optimizer to change p.DCOST = 0.1 #smooth out gas pedal movement p.DMAX = 20 #slow down change of gas pedal #CV tuning
tf = 200 m.time = np.linspace(0, tf, 2 * tf + 1) step = np.zeros(2 * tf + 1) step[3:40] = 2.0 step[40:] = 5.0 # Controller model Kc = 15.0 # controller gain tauI = 2.0 # controller reset time tauD = 1.0 # derivative constant OP_0 = m.Const(value=0.0) # OP bias OP = m.Var(value=0.0) # controller output PV = m.Var(value=0.0) # process variable SP = m.Param(value=step) # set point Intgl = m.Var(value=0.0) # integral of the error err = m.Intermediate(SP - PV) # set point error m.Equation(Intgl.dt() == err) # integral of the error m.Equation(OP == OP_0 + Kc * err + (Kc / tauI) * Intgl - PV.dt()) # Process model Kp = 0.5 # process gain tauP = 10.0 # process time constant m.Equation(tauP * PV.dt() + PV == Kp * OP) m.options.IMODE = 4 m.solve(disp=False) plt.figure() plt.subplot(2, 1, 1) plt.plot(m.time, OP.value, 'b:', label='OP') plt.ylabel('Output')
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()
alpha = m.MV(value=0.0, lb=-20 * d2r, ub=20 * d2r) alpha.STATUS = 1 x = m.Var(value=0., lb=0.) y = m.Var(value=0., lb=0.) v = m.Var(value=0., lb=0.) # , ub=1000.) gam = m.Var(value=89.9 * d2r, lb=-180. * d2r, ub=180. * d2r) mass = m.Var(value=m0)#, lb=m0-mp0, ub=m0) tave = mp0 * Isp * g / tsp step = [0 if z > tsp else tave for z in t_array * tf.value] ft = m.Param(value=step) rho = m.Intermediate( 1.22 * m.exp(-0.000091 * y + -1.88e-9 * y**2 )) q = m.Intermediate(0.5 * rho * v ** 2.) cd0 = m.Var(value=0.02) cdf = m.Var(value=0.5) cna = m.Var(value=0.2) cs = m.Intermediate(-0.00117 * y + 340.288) mn = m.Var(value=0.0) m.Equation(mn == v / cs) t_mn = [0.3, 0.5, 0.9, 1.2, 1.5, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0] t_cd0 = [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] t_cdf = [0.5, 0.6, 0.7, 0.8, 0.65, 0.6, 0.5, 0.4, 0.3, 0.25, 0.2] t_cna = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
vp = np.ones(n) * 10 vp[3:6] = -5 v = m.Param(value=vp) # define distance x = m.Var(20) # define data transmission rate dtr = m.MV(lb=0, ub=100) dtr.STATUS = 1 # define how much data must be transmitted data = m.Var(500) # energy to transmit e = m.Intermediate(dtr * x**2 * 1e-5) # equations # track the position m.Equation(x.dt() == v) # as data is transmitted, remaining data stored decreases m.Equation(data.dt() == -dtr) # objective m.Obj(e) # minimize energy # soft (objective constraint) final = m.Param(value=np.zeros(n)) final.value[-1] = 1