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
