Related
I'm new to both GEKKO and Python so bear with me. I'm trying to do a control optimization problem of a system of the general form:
Equations of Motion
This requires that I solve the 2nd-order terms as either Intermediates or Equations. However, I'm really struggling with how to express this correctly. I've read elsewhere that Gekko objects do not accept np.linalg.invbut can solve these equations when expressed implicitly. The below code attempts to express the .dt() terms implicitly, but the solver provides the below error message:
Exception: #error: Intermediate Definition
Error: Intermediate variable with no equality (=) expression
[((i3)*(cos(v3)))0((i6)*(cos((v3-v5))))]
STOPPING...
The above message (I believe) is referring to the M matrix. It's unclear to me if this is an issue with the formulation of my Intermediates or with the Equations.
Note: in this case, I could workaround this issue by explicitly expressing the 2nd-order terms, but even for such a simple system, those equations are rather large. For higher complexity systems, that won't be practical, so I'd really appreciate a method to solve the equations of motion in matrix form.
Much appreciated.
import math
import matplotlib
matplotlib.use("TkAgg")
import numpy as np
from gekko import GEKKO
#Defining a model
m = GEKKO()
#################################
#Define initial and final conditions and limits
x0 = -1; xdot0 = 0
q10 = 0; q1dot0 = 0
q20 = 0; q2dot0 = 0
xf = 1; xdotf = 0
q1f = 0; q1dotf = 0
q2f = 0; q2dotf = 0
xmin = -1.5; xmax = 1.5
umin = -10; umax = 10
#Defining the time parameter (0, 1)
N = 100
t = np.linspace(0,1,N)
m.time = t
#Final time
TF = m.FV(3,lb=2,ub=25); TF.STATUS = 1
end_loc = len(m.time)-1
#Parameters
mc = m.Param(value=1) #cart mass
m1 = m.Param(value=.01) #link 1 mass
m2 = m.Param(value=.01) #link 2 mass
L1 = m.Param(value=1) #link 1 length
LC1 = m.Param(value=.5) #link 1 CM pos
L2 = m.Param(value=1) #link 1 length
LC2 = m.Param(value=.5) #link 1 CM pos
I1 = m.Param(value=.01) #link 1 MOI
I2 = m.Param(value=.01) #link 2 MOI
g = m.Const(value=9.81) #gravity
pi = math.pi; pi = m.Const(value=pi)
#MV
u = m.MV(value=0,lb=umin,ub=umax); u.STATUS = 1
#State Variables
x, xdot, q1, q1dot, q2, q2dot = m.Array(m.Var, 6)
x.value = x0; xdot.value = xdot0
q1.value = q10; q1dot.value = q1dot0
q2.value = q20; q2dot.value = q2dot0
x.LOWER = xmin; x.UPPER = xmax
#Intermediates
h1 = m.Intermediate(mc + m1 + m2)
h2 = m.Intermediate(m1*LC1 + m2*L1)
h3 = m.Intermediate(m2*LC2)
h4 = m.Intermediate(m1*LC1**2 + m2*L1**2 + I1)
h5 = m.Intermediate(m2*LC2*L1)
h6 = m.Intermediate(m2*LC2**2 + I2)
h7 = m.Intermediate(m1*LC1*g + m2*L1*g)
h8 = m.Intermediate(m2*LC2*g)
M = m.Intermediate(np.array([[h1, h2*m.cos(q1), h3*m.cos(q2)],
[h2*m.cos(q1), h4, h5*m.cos(q1-q2)],
[h3*m.cos(q2), h5*m.cos(q1-q2), h6]]))
C = m.Intermediate(np.array([[0, -h2*q1dot*m.sin(q1), -h3*q2dot*m.sin(q2)],
[0, 0, h5*q2dot*m.sin(q1-q2)],
[0, -h5*q1dot*m.sin(q1-q2), 0]]))
G = m.Intermediate(np.array([[0], [-h7*m.sin(q1)], [-h8*m.sin(q2)]]))
U = m.Intermediate(np.array([[u], [0], [0]]))
DQ = m.Intermediate(np.array([[xdot], [q1dot], [q2dot]]))
CDQ = m.Intermediate(C*DQ)
#Defining the State Space Model
m.Equation(M*np.array([[xdot.dt()/TF], [q1dot.dt()/TF], [q2dot.dt()/TF]]) == U - CDQ - G)
m.Equation(x.dt()/TF == xdot)
m.Equation(q1.dt()/TF == q1dot)
m.Equation(q2.dt()/TF == q2dot)
#Defining final condition
m.fix(x,pos=end_loc,val=xf)
m.fix(xdot,pos=end_loc,val=xdotf)
m.fix(q1,pos=end_loc,val=q1f)
m.fix(q1dot,pos=end_loc,val=q1dotf)
m.fix(q2,pos=end_loc,val=q2f)
m.fix(q2dot,pos=end_loc,val=q2dotf)
#Try to minimize final time
m.Obj(TF)
m.options.IMODE = 6 #MPC
m.solve() #(disp=False)
m.time = np.multiply(TF, m.time)
print('Final time: ', TF.value[0])
A few things to modify:
Use np.array() without the m.Intermediate() to define arrays.
Use M#b or np.dot(M,b) for the dot product, not M*b.
Use np.array([]) for the 3x1 array definitions instead of np.array([[],[],[]]). The dot product understands the intent.
Use m.fix_final() to fix a final point. You may also consider softening the final constraint with the strategies shown in the Inverted Pendulum problem, especially if the solver isn't able to find a feasible solution.
Use m.GEKKO(remote=False) to solve locally, with no internet connection. Using the public server is also fine, but the local option may be faster and more reliable later when you are solving larger problems.
Here is the modified Python Gekko code:
import math
import matplotlib
matplotlib.use("TkAgg")
import numpy as np
from gekko import GEKKO
#Defining a model
m = GEKKO()
#################################
#Define initial and final conditions and limits
x0 = -1; xdot0 = 0
q10 = 0; q1dot0 = 0
q20 = 0; q2dot0 = 0
xf = 1; xdotf = 0
q1f = 0; q1dotf = 0
q2f = 0; q2dotf = 0
xmin = -1.5; xmax = 1.5
umin = -10; umax = 10
#Defining the time parameter (0, 1)
N = 100
t = np.linspace(0,1,N)
m.time = t
#Final time
TF = m.FV(3,lb=2,ub=25); TF.STATUS = 1
#Parameters
mc = m.Param(value=1) #cart mass
m1 = m.Param(value=.01) #link 1 mass
m2 = m.Param(value=.01) #link 2 mass
L1 = m.Param(value=1) #link 1 length
LC1 = m.Param(value=.5) #link 1 CM pos
L2 = m.Param(value=1) #link 1 length
LC2 = m.Param(value=.5) #link 1 CM pos
I1 = m.Param(value=.01) #link 1 MOI
I2 = m.Param(value=.01) #link 2 MOI
g = m.Const(value=9.81) #gravity
pi = math.pi; pi = m.Const(value=pi)
#MV
u = m.MV(value=0,lb=umin,ub=umax); u.STATUS = 1
#State Variables
x, xdot, q1, q1dot, q2, q2dot = m.Array(m.Var, 6)
x.value = x0; xdot.value = xdot0
q1.value = q10; q1dot.value = q1dot0
q2.value = q20; q2dot.value = q2dot0
x.LOWER = xmin; x.UPPER = xmax
#Intermediates
h1 = m.Intermediate(mc + m1 + m2)
h2 = m.Intermediate(m1*LC1 + m2*L1)
h3 = m.Intermediate(m2*LC2)
h4 = m.Intermediate(m1*LC1**2 + m2*L1**2 + I1)
h5 = m.Intermediate(m2*LC2*L1)
h6 = m.Intermediate(m2*LC2**2 + I2)
h7 = m.Intermediate(m1*LC1*g + m2*L1*g)
h8 = m.Intermediate(m2*LC2*g)
M = np.array([[h1, h2*m.cos(q1), h3*m.cos(q2)],
[h2*m.cos(q1), h4, h5*m.cos(q1-q2)],
[h3*m.cos(q2), h5*m.cos(q1-q2), h6]])
C = np.array([[0, -h2*q1dot*m.sin(q1), -h3*q2dot*m.sin(q2)],
[0, 0, h5*q2dot*m.sin(q1-q2)],
[0, -h5*q1dot*m.sin(q1-q2), 0]])
G = np.array([0, -h7*m.sin(q1), -h8*m.sin(q2)])
U = np.array([u, 0, 0])
DQ = np.array([xdot, q1dot, q2dot])
CDQ = C#DQ
b = np.array([xdot.dt()/TF, q1dot.dt()/TF, q2dot.dt()/TF])
Mb = M#b
#Defining the State Space Model
m.Equations([(Mb[i] == U[i] - CDQ[i] - G[i]) for i in range(3)])
m.Equation(x.dt()/TF == xdot)
m.Equation(q1.dt()/TF == q1dot)
m.Equation(q2.dt()/TF == q2dot)
#Defining final condition
m.fix_final(x,val=xf)
m.fix_final(xdot,val=xdotf)
m.fix_final(q1,val=q1f)
m.fix_final(q1dot,val=q1dotf)
m.fix_final(q2,val=q2f)
m.fix_final(q2dot,val=q2dotf)
#Try to minimize final time
m.Minimize(TF)
m.options.IMODE = 6 #MPC
m.solve() #(disp=False)
m.time = np.multiply(TF, m.time)
print('Final time: ', TF.value[0])
This gives a successful solution with 35 iterations of the IPOPT solver:
EXIT: Optimal Solution Found.
The solution was found.
The final value of the objective function is 364.520472987953
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 1.57619999999588 sec
Objective : 364.520472987953
Successful solution
---------------------------------------------------
Final time: 3.6819305146
I am trying to implement an MPC algorithm using GEKKO MPC to compare against my own MPC implementation. I have managed to perform the set point changes without a problem however, when I try to include steps in a disturbance the controlled variable does not return to steady state (a value of zero for deviation variables). This would make me think that I need to include a command which allows feedback? I am unsure.
from gekko import GEKKO
import numpy
import matplotlib.pyplot as plt
t = numpy.linspace(0,300,100)
m = GEKKO(remote = False)
m.time = t
# Parameters
T1 = m.Param(value = 53.97272679974334)
T2 = m.Param(value = 48.06851424706475)
T3 = m.Param(value = 38.48651254747577)
T4 = m.Param(value = 31.018933652439845)
k1 = m.Param(value = 5.51)
k2 = m.Param(value = 6.58)
γ1bar = m.Param(value = 0.333)
γ2bar = m.Param(value = 0.307)
A1 = m.Param(value = 730)
A2 = m.Param(value = 730)
A3 = m.Param(value = 730)
A4 = m.Param(value = 730)
v1bar = m.Param(value = 60)
v2bar = m.Param(value = 60)
# Manipulated variable
v1 = m.MV(value=0, lb=0, ub=100)
v1.STATUS = 1
#v1.DCOST = 0.1
#v2.DMAX = 20
v2 = m.MV(value=0, lb=0, ub=100)
v2.STATUS = 1
#v2.DCOST = 0.1
#v2.DMAX = 20
γ1 = m.MV(value=0, lb=0, ub=1)
γ1.STATUS = 1
#γ1.DCOST = 0.1
#v2.DMAX = 20
γ2 = m.MV(value=0, lb=0, ub=1)
γ2.STATUS = 1
#γ2.DCOST = 0.1
#v2.DMAX = 20
d = numpy.ones(t.shape)
d[0:10] = 0
d22 = numpy.zeros(t.shape)
d1 = m.Param(name='d1', value=d) # Disturbance
d2 = m.Param(name='d2', value=d22) # Disturbance
m.options.CV_TYPE = 2 # squared error
# Controlled Variable
h1 = m.CV(value=0)
h1.STATUS = 1 # add the SP to the objective
h1.SP = 1 # set point
h1.TR_INIT = 1 # set point trajectory
h1.TAU = 1 # time constant of trajectory
h1.BIAS = 1
h1.FSTATUS = 1
h2 = m.CV(value=0)
h2.STATUS = 1 # add the SP to the objective
h2.SP = 0 # set point
h2.TR_INIT = 1 # set point trajectory
h2.TAU = 1 # time constant of trajectory
h2.BIAS = 1
h2.FSTATUS = 1
h3 = m.CV(value=0)
h3.STATUS = 1 # add the SP to the objective
h3.SP = 0 # set point
h3.TR_INIT = 1 # set point trajectory
h3.TAU = 1 # time constant of trajectory
h3.BIAS = 1
h3.FSTATUS = 1
h4 = m.CV(value=0)
h4.STATUS = 1 # add the SP to the objective
h4.SP = 0 # set point
h4.TR_INIT = 1 # set point trajectory
h4.TAU = 1 # time constant of trajectory
h4.BIAS = 1
h4.FSTATUS = 1
m.Equation(h1.dt() == -(1/T1)*h1 + (A3/(A1*T3))*h3 + (γ1bar*k1*v1)/A1 + (γ1*k1*v1bar)/A1)
m.Equation(h2.dt() == -(1/T2)*h2 + (A4/(A2*T4))*h4 + (γ2bar*k2*v2)/A2 + (γ2*k2*v2bar)/A2)
m.Equation(h3.dt() == -(1/T3)*h3 + ((1-γ2bar)*k2*v2)/A3 - k2*v2bar*γ2/A3 - (k1*d1)/A3)
m.Equation(h4.dt() == -(1/T4)*h4 + ((1-γ1bar)*k1*v1)/A4 - k1*v1bar*γ1/A4 - (k2*d2)/A4)
m.options.IMODE = 6 # control
m.solve(disp=False,debug = True)
Try setting h1.SP = 0 instead of 1 to set the objective to drive it back to 1. Also, these options are not needed if h4.TR_INIT = 0 to create a setpoint that is zero everywhere (not a reference trajectory).
h4.TAU = 1
h4.BIAS = 1
h4.FSTATUS = 1
I also added lower bounds of zero for each of the height CVs with lb=0. There is a move on v2 when the disturbance starts. It appears that the controller can't resolve this offset. Is this the correct response?
from gekko import GEKKO
import numpy
import matplotlib.pyplot as plt
t = numpy.linspace(0,300,100)
m = GEKKO(remote=False)
m.time = t
T1 = m.Param(value = 53.97272679974334)
T2 = m.Param(value = 48.06851424706475)
T3 = m.Param(value = 38.48651254747577)
T4 = m.Param(value = 31.018933652439845)
k1 = m.Param(value = 5.51)
k2 = m.Param(value = 6.58)
γ1bar = m.Param(value = 0.333)
γ2bar = m.Param(value = 0.307)
A1 = m.Param(value = 730)
A2 = m.Param(value = 730)
A3 = m.Param(value = 730)
A4 = m.Param(value = 730)
v1bar = m.Param(value = 60)
v2bar = m.Param(value = 60)
# Manipulated variable
v1 = m.MV(value=0, lb=0, ub=100)
v1.STATUS = 1
#v1.DCOST = 0.1
#v2.DMAX = 20
v2 = m.MV(value=0, lb=0, ub=100)
v2.STATUS = 1
#v2.DCOST = 0.1
#v2.DMAX = 20
γ1 = m.MV(value=0, lb=0, ub=1)
γ1.STATUS = 1
#γ1.DCOST = 0.1
#v2.DMAX = 20
γ2 = m.MV(value=0, lb=0, ub=1)
γ2.STATUS = 1
#γ2.DCOST = 0.1
#v2.DMAX = 20
d = numpy.ones(t.shape)
d[0:10] = 0
d22 = numpy.zeros(t.shape)
d1 = m.Param(name='d1', value=d) # Disturbance
d2 = m.Param(name='d2', value=d22) # Disturbance
m.options.CV_TYPE = 2 # squared error
# Controlled Variable
h1 = m.CV(value=0,lb=0)
h1.STATUS = 1 # add the SP to the objective
h1.SP = 0 # set point
h1.TR_INIT = 0 # set point trajectory
#h1.TAU = 1 # time constant of trajectory
#h1.BIAS = 1
#h1.FSTATUS = 1
h2 = m.CV(value=0,lb=0)
h2.STATUS = 1 # add the SP to the objective
h2.SP = 0 # set point
h2.TR_INIT = 0 # set point trajectory
#h2.TAU = 1 # time constant of trajectory
#h2.BIAS = 1
#h2.FSTATUS = 1
h3 = m.CV(value=0,lb=0)
h3.STATUS = 1 # add the SP to the objective
h3.SP = 0 # set point
h3.TR_INIT = 0 # set point trajectory
#h3.TAU = 1 # time constant of trajectory
#h3.BIAS = 1
#h3.FSTATUS = 1
h4 = m.CV(value=0,lb=0)
h4.STATUS = 1 # add the SP to the objective
h4.SP = 0 # set point
h4.TR_INIT = 0 # set point trajectory
#h4.TAU = 1 # time constant of trajectory
#h4.BIAS = 1
#h4.FSTATUS = 1
m.Equation(h1.dt() == -(1/T1)*h1 + (A3/(A1*T3))*h3 + (γ1bar*k1*v1)/A1 + (γ1*k1*v1bar)/A1)
m.Equation(h2.dt() == -(1/T2)*h2 + (A4/(A2*T4))*h4 + (γ2bar*k2*v2)/A2 + (γ2*k2*v2bar)/A2)
m.Equation(h3.dt() == -(1/T3)*h3 + ((1-γ2bar)*k2*v2)/A3 - k2*v2bar*γ2/A3 - (k1*d1)/A3)
m.Equation(h4.dt() == -(1/T4)*h4 + ((1-γ1bar)*k1*v1)/A4 - k1*v1bar*γ1/A4 - (k2*d2)/A4)
m.options.IMODE = 6 # control
m.solve(disp=True)
plt.subplot(3,1,1)
plt.plot(m.time,d1.value,'k-',label='Disturbance')
plt.ylabel('Disturbance'); plt.grid(); plt.legend()
plt.subplot(3,1,2)
plt.plot(m.time,h1.value,'r-',label='Height 1')
plt.plot(m.time,h2.value,'g--',label='Height 2')
plt.plot(m.time,h3.value,'b:',label='Height 3')
plt.plot(m.time,h3.value,'k--',alpha=0.5,label='Height 4')
plt.ylabel('CVs'); plt.grid(); plt.legend()
plt.subplot(3,1,3)
plt.plot(m.time,v1.value,'r-',label='MV 1a')
plt.plot(m.time,v2.value,'g--',label='MV 2a')
plt.plot(m.time,γ1.value,'b:',label='MV 1b')
plt.plot(m.time,γ2.value,'k--',alpha=0.5,label='MV 2b')
plt.ylabel('MVs'); plt.grid(); plt.legend()
plt.show()
everyone and Professor John
We are using gekko to do MPC on tclab simulation model. We try to emulate the situation that on site the actuator deviates from MV calculated by gekko because of the problems of actuator.
If the deviation is in the fixed pattern, for example a quite big constant deviation happens for a long time and may come back then work well for a long time. We can deal with it by extra logic to detect deviation and add the deviation value to the mv calculated by gekko.
one day, I noticed that there could be meas for MV when fstatus = 1. So I gave it a try. I hope gekko could deal with the deviation by itself. for example, if mv from gekko is 10 and the measurement is 5 and the pattern continues, gekko may spit out a higher MV value than 10, for example 15 and measurement is 10.
In the simulation, when I set MV's fstatus=1, the MV's curve becomes to :
q1a is the q1 with manual deviation. In the above pic, q1a == q1. It looks like gekko takes one more step thinking about the MV's effect.
In the below pic, there are two times range, one with "q1a == q1+20" and the other with "q1a == q1 -20". q1a's value is fed to tclab and mv(q1)'s meas.
I do not understand why the q1 calculated by gekko is going up or going down when meas deviates despite the t1 is going far away from sp.
Edit: Example Code
See the screen shot below from "normal" HMI. The sluggish MV disappeared, so it maybe caused by bug in my code. But the up-going or down-going could still be seen.
See my code below:
from random import random
from random import randrange
import tclab
from tclab import labtime
from tclab import TCLabModel
import numpy as np
import time
import matplotlib.pyplot as plt
from gekko import GEKKO
import json
from tclab import TCLabModel
make_mp4 = True
if make_mp4:
import imageio # required to make animation
import os
try:
os.mkdir('./figures')
except:
pass
class tclab_heaterpipe():
def __init__(self,d1,d2,model):
if(d1 >= 1 and d2 >=1):
self.delay_q1_step = int(d1)
self.delay_q2_step = int(d2)
self.q1_buffer = [0] * self.delay_q1_step
self.q2_buffer = [0] * self.delay_q2_step
self.m = model
else:
self.delay_q1_step =0
self.delay_q2_step =0
return
def Q1_delay(self,q1):
if(self.delay_q1_step == 0):
self.m.Q1(q1)
self.q1_buffer.insert(0,q1)
self.m.Q1(self.q1_buffer.pop())
def Q2_delay(self,q2):
if(self.delay_q2_step == 0):
self.m.Q1(q2)
self.q2_buffer.insert(0,q2)
self.m.Q2(self.q2_buffer.pop())
# Connect to Arduino
connected = False
theta1 = 1
theta2 = 1
T = tclab.setup(connected)
a = T()
tclab_delay = tclab_heaterpipe(theta1,theta2,a)
# Turn LED on
print('LED On')
a.LED(100)
# Simulate a time delay
# Run time in minutes
run_time = 80.0
# Number of cycles
loops = int(60.0*run_time)
# Temperature (K)
t1sp = 45.0
t2sp = 35.0
#########################################################
# Initialize Model
#########################################################
# use remote=True for MacOS
m = GEKKO(name='tclab-mpc',remote=False)
m.time = np.linspace(0,400,41)
step = 10
T1 = np.ones(int(loops/step)+1) * a.T1 # temperature (degC)
T2 = np.ones(int(loops/step)+1) * a.T2 # temperature (degC)
Tsp1 = np.ones(int(loops/step)+1) * t1sp # set point (degC)
Tsp2 = np.ones(int(loops/step)+1) * t2sp # set point (degC)
# heater values
Q1s = np.ones(int(loops/step)+1) * 0.0
Q2s = np.ones(int(loops/step)+1) * 0.0
# Parameters
Q1_ss = m.Param(value=0)
TC1_ss = m.Param(value=a.T1)
Q2_ss = m.Param(value=0)
TC2_ss = m.Param(value=a.T2)
Kp1 = m.Param(value= 0.7)
tau1 = m.Param(value=160.0)
Kp2 = m.Param(value=0.05)
tau2 = m.Param(value=160.0)
Kp3= m.Param(value=0.05)
tau3 = m.Param(value=160.0)
Kp4 = m.Param(value=0.4)
tau4 = m.Param(value=200.0)
sp1 = m.Param(value=a.T1)
sp2 = m.Param(value=a.T2)
# Manipulated variable
Q1 = m.MV(value=0, name='q1')
Q1.STATUS = 1 # use to control temperature
Q1.FSTATUS = 1 # no feedback measurement
Q1.LOWER = 0.0
Q1.UPPER = 100.0
Q1.DMAX = 10.0
Q1.DCOST = 5.0
Q2 = m.MV(value=0, name='q2')
Q2.STATUS = 1 # use to control temperature
Q2.FSTATUS = 1 # no feedback measurement
Q2.LOWER = 0.0
Q2.UPPER = 100.0
Q2.DMAX = 10.0
Q2.DCOST = 5.0
# Controlled variable
TC1 = m.CV(value=a.T1, name='tc1')
TC1.STATUS = 1 # minimize error with setpoint range
TC1.FSTATUS = 1 # receive measurement
TC1.TR_INIT = 2 # reference trajectory
# TC1.COST = 0.1
TC1.WSPHI = 20
TC1.WSPLO = 20
TC1.TAU = 50 # time constant for response
#TC1.TR_OPEN = 3
TC2 = m.CV(value=a.T2, name='tc2')
TC2.STATUS = 1 # minimize error with setpoint range
TC2.FSTATUS = 1 # receive measurement
TC2.TR_INIT = 2 # reference trajectory
# TC2.COST = 0.1
TC2.WSPHI = 20
TC2.WSPLO = 20
TC2.TAU = 30 # time constant for response
#kTC2.TR_OPEN = 3
# 添加延时
Q1d=m.Var()
m.delay(Q1, Q1d, theta1)
Q2d=m.Var()
m.delay(Q2, Q2d, theta2)
# Equation
#m.Equation(tau1 * TC1.dt() + (TC1 - TC1_ss) == Kp1 * (Q1d - Q1_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss) == Kp2 * (Q1d - Q1_ss))
# m.Equation(tau3 * TC1.dt() + (TC1 - TC1_ss) == Kp3 * (Q2d - Q2_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss) == Kp4 * (Q2d - Q2_ss))
m.Equation(0.5 * (tau1 * TC1.dt() + (TC1 - TC1_ss) + tau3 * TC1.dt() + (TC1 - TC1_ss)) == Kp1 * (Q1d - Q1_ss) + Kp3 * (Q2d -Q2_ss))
m.Equation(0.5 * (tau2 * TC2.dt() + (TC2 - TC2_ss) + tau4 * TC2.dt() + (TC2 - TC2_ss)) == Kp4 * (Q2d - Q2_ss) + Kp2 * (Q1d - Q1_ss))
# Steady-state initializations
m.options.IMODE = 1
m.options.SOLVER = 1 # 1=APOPT, 3=IPOPT
m.solve()
sp1.VALUE = 45
sp2.VALUE = 35
# Global Options
m.options.IMODE = 6 # MPC
m.options.CV_TYPE = 3 # Objective type
m.options.NODES = 2 # Collocation nodes
m.options.MAX_TIME = 10
m.options.SOLVER = 1 # 1=APOPT, 3=IPOPT
#m.options.CV_WGT_START = 2*theta
#m.options.CV_WGT_SLOPE = theta
# m.options.MV_STEP_HOR = 5
##################################################################
# Create plot
plt.figure()
plt.ion()
plt.show()
# Main Loop
a.Q1(0)
a.Q2(0)
Q2s[0:] = 0
start_time = time.time()
tm = np.linspace(1,loops,int(loops/step)+1)
j=0
try:
time_start = time.time()
labtime_start = labtime.time()
if(not connected):
labtime.set_rate(10)
for i in tclab.clock(loops,adaptive=False):
i = int(i)
if(i == 0):
continue
print("-----------------------")
t_real = time.time() - time_start
t_lab = labtime.time() - labtime_start
print("real time = {0:4.1f} lab time = {1:4.1f} m.time = {1:4.1f}".format(t_real, t_lab,m.time))
#print("real time = {0:4.1f} m.time = {1:4.1f}".format(t_real, m.time))
if(i%step != 0):
continue
j = i/step
j = int(j)
print(j)
T1[j:] = a.T1
T2[j:] = a.T2
tm[j] = i
###############################
### MPC CONTROLLER ###
###############################
TC1.MEAS = T1[j]
TC2.MEAS = T2[j]
print("T1 meas:{0:4.1f} ".format(a.T1))
print("T2 meas:{0:4.1f} ".format(a.T2))
# input setpoint with deadband +/- DT
DT =0.5
TC1.SPHI = Tsp1[j] +DT
TC1.SPLO = Tsp1[j] -DT
TC2.SPHI = Tsp2[j] +DT
TC2.SPLO = Tsp2[j] -DT
try:
# stop model time to solve MPC in cast the solver takes too much time
if(not connected):
labtime.stop()
m.solve(disp=False)
#start model time
if(not connected):
labtime.start()
except Exception as e:
if(not connected):
if(not labtime.running):
labtime.start()
print("sovle's exception:")
print(e)
if(j != 0):
Q1s[j] = Q1s[j-1]
Q2s[j] = Q2s[j-1]
continue
# test for successful solution
if (m.options.APPSTATUS==1):
# retrieve the first Q value
Q1s[j:] = np.ones(len(Q1s)-j) * Q1.NEWVAL
Q2s[j:] = np.ones(len(Q2s)-j) * Q2.NEWVAL
#a.Q1(Q1.NEWVAL)
#a.Q2(Q2.NEWVAL)
print("Q1 applied with delay: {0:4.1f} ".format(Q1.NEWVAL))
print("Q2 applied with delay: {0:4.1f} ".format(Q2.NEWVAL))
with open(m.path+'//results.json') as f:
results = json.load(f)
else:
# not successful, set heater to zero
print("APPSTATUS is not 1,set Q to 0")
#Q1s[j] = 0
#Q2s[j] = 0
if i> 300 and i < 600:
Q1s[j] = Q1s[j] - 20
Q2s[j] = Q2s[j] - 20
if i>= 600:
Q1s[j] = Q1s[j] + 20
Q2s[j] = Q2s[j] + 20
Q1.meas= Q1s[j]
Q2.meas= Q2s[j]
tclab_delay.Q1_delay(Q1s[j])
tclab_delay.Q2_delay(Q2s[j])
print("calc:"+str(Q1s[j]))
print("calc:"+str(Q2s[j]))
#apply disturbance on 50s, 200s,
#if(i == 600):
# Q2s[j] = 100
#if(i == 1400):
# Q2s[j] = 0
#Q2s[j] = 20 - randrange(20)
#Q2s[j:] = np.ones(len(Q2s)-j) * Q2s[j]
#restore Q2 to 0
#if(i == 300):
#Q2s[j:] = 0
#a.Q2(Q2s[j])
#tclab_delay.Q2_delay(Q2s[j])
#take Q2 to FV
#Q2.MEAS = Q2s[j]
if(not connected):
labtime.stop()
# Plot
try:
plt.clf()
ax=plt.subplot(2,1,1)
ax.grid()
plt.plot(tm[0:j],T1[0:j],'ro',markersize=3,label=r'$T_1$')
plt.plot(tm[0:j],Tsp1[0:j],'r-',markersize=3,label=r'$T_1 Setpoint$')
plt.plot(tm[0:j],T2[0:j],'bo',markersize=3,label=r'$T_2$')
plt.plot(tm[0:j],Tsp2[0:j],'b-',markersize=3,label=r'$T_2 Setpoint$')
plt.plot(tm[j]+m.time,results['tc1.bcv'],'r-.',markersize=1,\
label=r'$T_1$ predicted',linewidth=1)
plt.plot(tm[j]+m.time,results['tc2.bcv'],'b-.',markersize=1,\
label=r'$T_2$ predicted',linewidth=1)
plt.plot(tm[j]+m.time,results['tc1.tr_hi'],'k--',\
label=r'$T_1$ trajectory')
plt.plot(tm[j]+m.time,results['tc1.tr_lo'],'k--')
plt.plot(tm[j]+m.time,results['tc2.tr_hi'],'k--',\
label=r'$T_2$ trajectory')
plt.plot(tm[j]+m.time,results['tc2.tr_lo'],'k--')
plt.ylabel('Temperature (degC)')
plt.legend(loc='best')
ax=plt.subplot(2,1,2)
ax.grid()
plt.plot(tm[0:j],Q1s[0:j],'r-',linewidth=3,label=r'$Q_1$')
plt.plot(tm[0:j],Q2s[0:j],'b-',linewidth=3,label=r'$Q_2$')
plt.plot(tm[j]+m.time,Q1.value,'r-.',\
label=r'$Q_1$ plan',linewidth=1)
plt.plot(tm[j]+m.time,Q2.value,'b-.',\
label=r'$Q_2$ plan',linewidth=1)
#plt.plot(tm[0:i],Q2s[0:i],'b:',LineWidth=3,label=r'$Q_2$')
plt.ylabel('Heaters')
plt.xlabel('Time (sec)')
plt.legend(loc='best')
plt.draw()
plt.pause(0.05)
if make_mp4:
filename='./figures/plot_'+str(j+10000)+'.png'
plt.savefig(filename)
except Exception as e:
print(e)
pass
if(not connected):
labtime.start()
# Turn off heaters
a.Q1(0)
a.Q2(0)
print('Shutting down')
input("Press Enter to continue...")
a.close()
# Allow user to end loop with Ctrl-C
except KeyboardInterrupt:
# Disconnect from Arduino
a.Q1(0)
a.Q2(0)
print('Shutting down')
a.close()
if make_mp4:
images = []
iset = 0
for i in range(1,int(loops/step)+1):
filename='./figures/plot_'+str(i+10000)+'.png'
if os.path.exists(filename):
images.append(imageio.imread(filename))
if ((i+1)%350)==0:
imageio.mimsave('results_'+str(iset)+'.mp4', images)
iset += 1
images = []
if images!=[]:
imageio.mimsave('results_'+str(iset)+'.mp4', images)
# Make sure serial connection still closes when there's an error
except:
# Disconnect from Arduino
a.Q1(0)
a.Q2(0)
print('Error: Shutting down')
a.close()
raise
Regards
Tibalt
Is the FSTATUS also ON for the CVs such as t1.FSTATUS=1? If you update the measurement such as:
t1.MEAS = lab.T1
t2.MEAS = lab.T2
then this updates the BIAS for t1 and t2 (BIAS documentation). This should take care of any process / model mismatch that you are introducing by arbitrarily increasing or decreasing the heater by 20%. If t1.FSTATUS is OFF (0) then it is not able to compensate for the mismatch.
Another thing to try is to adjust the reference trajectory. The controller can appear sluggish if TAU is too high. Here is an example application with MPC and a linear model.
One additional way to compensate for the mismatch is to use Moving Horizon Estimation as shown here.
It looks like you have created a nice interface!
Response to Edit
Thanks for adding the code. The problem is that Q1.DMAX=10 and Q2.DMAX=10. When the Q1 and Q2 values are shifted up by 20 each cycle, the most that the controller can shift down is 20-10=10 so the controller appears that it is ramping in the wrong direction. Changing to DMAX=100 fixes the problem. There is still offset from the setpoint because the recommended Q1 and Q2 are shifted each cycle. The true recommended values are never implemented. Another thing to try is to impose an offset on the measured values such as TC1.MEAS = T1[j] + 20. The model bias will remove the offset in this case.
from random import random
from random import randrange
import tclab
from tclab import labtime
from tclab import TCLabModel
import numpy as np
import time
import matplotlib.pyplot as plt
from gekko import GEKKO
import json
from tclab import TCLabModel
make_gif = True
make_mp4 = True
if make_gif or make_mp4:
# pip install imageio-ffmpeg with imageio to make MP4
import imageio # required to make animation
import os
try:
os.mkdir('./figures')
except:
pass
class tclab_heaterpipe():
def __init__(self,d1,d2,model):
if(d1 >= 1 and d2 >=1):
self.delay_q1_step = int(d1)
self.delay_q2_step = int(d2)
self.q1_buffer = [0] * self.delay_q1_step
self.q2_buffer = [0] * self.delay_q2_step
self.m = model
else:
self.delay_q1_step =0
self.delay_q2_step =0
return
def Q1_delay(self,q1):
if(self.delay_q1_step == 0):
self.m.Q1(q1)
self.q1_buffer.insert(0,q1)
self.m.Q1(self.q1_buffer.pop())
def Q2_delay(self,q2):
if(self.delay_q2_step == 0):
self.m.Q1(q2)
self.q2_buffer.insert(0,q2)
self.m.Q2(self.q2_buffer.pop())
# Connect to Arduino
connected = False # switch to connected=True with physical hardware
theta1 = 1
theta2 = 1
T = tclab.setup(connected)
a = T()
tclab_delay = tclab_heaterpipe(theta1,theta2,a)
# Turn LED on
print('LED On')
a.LED(100)
# Simulate a time delay
# Run time in minutes
run_time = 20.0
# Number of cycles
loops = int(60.0*run_time)
# Temperature (K)
t1sp = 45.0
t2sp = 35.0
#########################################################
# Initialize Model
#########################################################
# use remote=True for MacOS
m = GEKKO(name='tclab-mpc',remote=False)
m.time = np.linspace(0,400,41)
step = 10
T1 = np.ones(int(loops/step)+1) * a.T1 # temperature (degC)
T2 = np.ones(int(loops/step)+1) * a.T2 # temperature (degC)
Tsp1 = np.ones(int(loops/step)+1) * t1sp # set point (degC)
Tsp2 = np.ones(int(loops/step)+1) * t2sp # set point (degC)
# heater values
Q1s = np.ones(int(loops/step)+1) * 0.0
Q2s = np.ones(int(loops/step)+1) * 0.0
# Parameters
Q1_ss = m.Param(value=0)
TC1_ss = m.Param(value=a.T1)
Q2_ss = m.Param(value=0)
TC2_ss = m.Param(value=a.T2)
Kp1 = m.Param(value= 0.7)
tau1 = m.Param(value=160.0)
Kp2 = m.Param(value=0.05)
tau2 = m.Param(value=160.0)
Kp3= m.Param(value=0.05)
tau3 = m.Param(value=160.0)
Kp4 = m.Param(value=0.4)
tau4 = m.Param(value=200.0)
sp1 = m.Param(value=a.T1)
sp2 = m.Param(value=a.T2)
# Manipulated variable
Q1 = m.MV(value=0, name='q1')
Q1.STATUS = 1 # use to control temperature
Q1.FSTATUS = 1 # no feedback measurement
Q1.LOWER = 0.0
Q1.UPPER = 100.0
Q1.DMAX = 100.0
Q1.DCOST = 1e-3
Q2 = m.MV(value=0, name='q2')
Q2.STATUS = 1 # use to control temperature
Q2.FSTATUS = 1 # no feedback measurement
Q2.LOWER = 0.0
Q2.UPPER = 100.0
Q2.DMAX = 100.0
Q2.DCOST = 1e-3
# Controlled variable
TC1 = m.CV(value=a.T1, name='tc1')
TC1.STATUS = 1 # minimize error with setpoint range
TC1.FSTATUS = 1 # receive measurement
TC1.TR_INIT = 2 # reference trajectory
# TC1.COST = 0.1
TC1.WSPHI = 20
TC1.WSPLO = 20
TC1.TAU = 50 # time constant for response
#TC1.TR_OPEN = 3
TC2 = m.CV(value=a.T2, name='tc2')
TC2.STATUS = 1 # minimize error with setpoint range
TC2.FSTATUS = 1 # receive measurement
TC2.TR_INIT = 2 # reference trajectory
# TC2.COST = 0.1
TC2.WSPHI = 20
TC2.WSPLO = 20
TC2.TAU = 30 # time constant for response
#kTC2.TR_OPEN = 3
# 添加延时
Q1d=m.Var()
m.delay(Q1, Q1d, theta1)
Q2d=m.Var()
m.delay(Q2, Q2d, theta2)
# Equation
#m.Equation(tau1 * TC1.dt() + (TC1 - TC1_ss) == Kp1 * (Q1d - Q1_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss) == Kp2 * (Q1d - Q1_ss))
# m.Equation(tau3 * TC1.dt() + (TC1 - TC1_ss) == Kp3 * (Q2d - Q2_ss))
# m.Equation(tau2 * TC2.dt() + (TC2 - TC2_ss) == Kp4 * (Q2d - Q2_ss))
m.Equation(0.5 * (tau1 * TC1.dt() + (TC1 - TC1_ss) + tau3 * TC1.dt() + (TC1 - TC1_ss)) == Kp1 * (Q1d - Q1_ss) + Kp3 * (Q2d -Q2_ss))
m.Equation(0.5 * (tau2 * TC2.dt() + (TC2 - TC2_ss) + tau4 * TC2.dt() + (TC2 - TC2_ss)) == Kp4 * (Q2d - Q2_ss) + Kp2 * (Q1d - Q1_ss))
# Steady-state initializations
m.options.IMODE = 1
m.options.SOLVER = 1 # 1=APOPT, 3=IPOPT
m.solve()
sp1.VALUE = 45
sp2.VALUE = 35
# Global Options
m.options.IMODE = 6 # MPC
m.options.CV_TYPE = 3 # Objective type
m.options.NODES = 2 # Collocation nodes
m.options.MAX_TIME = 10
m.options.SOLVER = 1 # 1=APOPT, 3=IPOPT
#m.options.CV_WGT_START = 2*theta
#m.options.CV_WGT_SLOPE = theta
# m.options.MV_STEP_HOR = 5
##################################################################
# Create plot
plt.figure(figsize=(12,8))
plt.ion()
plt.show()
# Main Loop
a.Q1(0)
a.Q2(0)
Q2s[0:] = 0
start_time = time.time()
tm = np.linspace(1,loops,int(loops/step)+1)
j=0
try:
time_start = time.time()
labtime_start = labtime.time()
if(not connected):
labtime.set_rate(10)
for i in tclab.clock(loops,adaptive=False):
i = int(i)
if(i == 0):
continue
print("-----------------------")
t_real = time.time() - time_start
t_lab = labtime.time() - labtime_start
print("real time = {0:4.1f} lab time = {1:4.1f} m.time = {1:4.1f}".format(t_real, t_lab,m.time))
#print("real time = {0:4.1f} m.time = {1:4.1f}".format(t_real, m.time))
if(i%step != 0):
continue
j = i/step
j = int(j)
print(j)
T1[j:] = a.T1
T2[j:] = a.T2
tm[j] = i
###############################
### MPC CONTROLLER ###
###############################
TC1.MEAS = T1[j]
TC2.MEAS = T2[j]
print("T1 meas:{0:4.1f} ".format(a.T1))
print("T2 meas:{0:4.1f} ".format(a.T2))
# input setpoint with deadband +/- DT
DT =0.5
TC1.SPHI = Tsp1[j] +DT
TC1.SPLO = Tsp1[j] -DT
TC2.SPHI = Tsp2[j] +DT
TC2.SPLO = Tsp2[j] -DT
try:
# stop model time to solve MPC in cast the solver takes too much time
if(not connected):
labtime.stop()
m.solve(disp=False)
#start model time
if(not connected):
labtime.start()
except Exception as e:
if(not connected):
if(not labtime.running):
labtime.start()
print("sovle's exception:")
print(e)
if(j != 0):
Q1s[j] = Q1s[j-1]
Q2s[j] = Q2s[j-1]
continue
# test for successful solution
if (m.options.APPSTATUS==1):
# retrieve the first Q value
Q1s[j:] = np.ones(len(Q1s)-j) * Q1.NEWVAL
Q2s[j:] = np.ones(len(Q2s)-j) * Q2.NEWVAL
#a.Q1(Q1.NEWVAL)
#a.Q2(Q2.NEWVAL)
print("Q1 applied with delay: {0:4.1f} ".format(Q1.NEWVAL))
print("Q2 applied with delay: {0:4.1f} ".format(Q2.NEWVAL))
with open(m.path+'//results.json') as f:
results = json.load(f)
else:
# not successful, set heater to zero
print("APPSTATUS is not 1,set Q to 0")
#Q1s[j] = 0
#Q2s[j] = 0
if i> 300 and i < 600:
Q1s[j] = max(0,Q1s[j] - 20)
Q2s[j] = max(0,Q2s[j] - 20)
if i>= 600:
Q1s[j] = min(100,Q1s[j] + 20)
Q2s[j] = min(100,Q2s[j] + 20)
Q1.meas= Q1s[j]
Q2.meas= Q2s[j]
tclab_delay.Q1_delay(Q1s[j])
tclab_delay.Q2_delay(Q2s[j])
print("calc:"+str(Q1s[j]))
print("calc:"+str(Q2s[j]))
if(not connected):
labtime.stop()
# Plot
try:
plt.clf()
ax=plt.subplot(2,1,1)
ax.grid()
plt.plot(tm[0:j],T1[0:j],'ro',markersize=3,label=r'$T_1$')
plt.plot(tm[0:j],Tsp1[0:j],'r-',markersize=3,label=r'$T_1 Setpoint$')
plt.plot(tm[0:j],T2[0:j],'bo',markersize=3,label=r'$T_2$')
plt.plot(tm[0:j],Tsp2[0:j],'b-',markersize=3,label=r'$T_2 Setpoint$')
plt.plot(tm[j]+m.time,results['tc1.bcv'],'r-.',markersize=1,\
label=r'$T_1$ predicted',linewidth=1)
plt.plot(tm[j]+m.time,results['tc2.bcv'],'b-.',markersize=1,\
label=r'$T_2$ predicted',linewidth=1)
plt.plot(tm[j]+m.time,results['tc1.tr_hi'],'k--',\
label=r'$T_1$ trajectory')
plt.plot(tm[j]+m.time,results['tc1.tr_lo'],'k--')
plt.plot(tm[j]+m.time,results['tc2.tr_hi'],'k--',\
label=r'$T_2$ trajectory')
plt.plot(tm[j]+m.time,results['tc2.tr_lo'],'k--')
plt.ylabel('Temperature (degC)')
plt.legend(loc=1)
ax=plt.subplot(2,1,2)
ax.grid()
plt.plot(tm[0:j],Q1s[0:j],'r-',linewidth=3,label=r'$Q_1$')
plt.plot(tm[0:j],Q2s[0:j],'b-',linewidth=3,label=r'$Q_2$')
plt.plot(tm[j]+m.time,Q1.value,'r-.',\
label=r'$Q_1$ plan',linewidth=1)
plt.plot(tm[j]+m.time,Q2.value,'b-.',\
label=r'$Q_2$ plan',linewidth=1)
#plt.plot(tm[0:i],Q2s[0:i],'b:',LineWidth=3,label=r'$Q_2$')
plt.ylabel('Heaters')
plt.xlabel('Time (sec)')
plt.legend(loc=1)
plt.draw()
plt.pause(0.05)
if make_mp4:
filename='./figures/plot_'+str(j+10000)+'.png'
plt.savefig(filename)
except Exception as e:
print(e)
pass
if(not connected):
labtime.start()
# Turn off heaters
a.Q1(0)
a.Q2(0)
print('Shutting down')
input("Press Enter to continue...")
a.close()
# make gif
if make_gif:
images = []
iset = 0
for i in range(1,int(loops/step)+1):
filename='./figures/plot_'+str(i+10000)+'.png'
if os.path.exists(filename):
images.append(imageio.imread(filename))
if ((i+1)%350)==0:
imageio.mimsave('results_'+str(iset)+'.gif', images)
iset += 1
images = []
if images!=[]:
imageio.mimsave('results_'+str(iset)+'.gif', images)
if make_mp4:
images = []
iset = 0
for i in range(1,int(loops/step)+1):
filename='./figures/plot_'+str(i+10000)+'.png'
if os.path.exists(filename):
images.append(imageio.imread(filename))
if ((i+1)%350)==0:
imageio.mimsave('results_'+str(iset)+'.gif', images)
iset += 1
images = []
if images!=[]:
imageio.mimsave('results_'+str(iset)+'.gif', images)
# Allow user to end loop with Ctrl-C
except KeyboardInterrupt:
# Disconnect from Arduino
a.Q1(0)
a.Q2(0)
print('Shutting down')
a.close()
if make_gif:
images = []
iset = 0
for i in range(1,int(loops/step)+1):
filename='./figures/plot_'+str(i+10000)+'.png'
if os.path.exists(filename):
images.append(imageio.imread(filename))
if ((i+1)%350)==0:
imageio.mimsave('results_'+str(iset)+'.gif', images)
iset += 1
images = []
if images!=[]:
imageio.mimsave('results_'+str(iset)+'.gif', images)
if make_mp4:
images = []
iset = 0
for i in range(1,int(loops/step)+1):
filename='./figures/plot_'+str(i+10000)+'.png'
if os.path.exists(filename):
images.append(imageio.imread(filename))
if ((i+1)%350)==0:
imageio.mimsave('results_'+str(iset)+'.mp4', images)
iset += 1
images = []
if images!=[]:
imageio.mimsave('results_'+str(iset)+'.mp4', images)
# Make sure serial connection still closes when there's an error
except:
# Disconnect from Arduino
a.Q1(0)
a.Q2(0)
print('Error: Shutting down')
a.close()
raise
I cannot seem to figure out why IPOPT cannot find a solution to this. Initially, I thought the problem was totally infeasible but when I reduce the value of col_total to any number below 161000 or comment out the last constraint equation that contains col_total, it solves and EXITs with an Optimal Solution Found and a final objective value function of -161775.256826753. I have solved the same Maximization problem using Artificial Bee Colony and Particle Swamp Optimization techniques, and they solve and return optimal objective value function at least 225000 and 226000 respectively. Could it be that another solver is required? I have also tried APOPT, BPOPT, and IPOPT and have tinkered around with the tolerance values, but no combination none seems to work just yet. The code is posted below. Any guidance will be hugely appreciated.
from gekko import GEKKO
import numpy as np
distances = np.array([[[0, 0],[0,0],[0,0],[0,0]],\
[[155,0],[0,0],[0,0],[0,0]],\
[[310,0],[155,0],[0,0],[0,0]],\
[[465,0],[310,0],[155,0],[0,0]],\
[[620,0],[465,0],[310,0],[155,0]]])
alpha = 0.5 / np.log(30/0.075)
diam = 31
free = 7
rho = 1.2253
area = np.pi * (diam / 2)**2
min_v = 5.5
axi_max = 0.32485226746
col_total = 176542.96546512868
rat = 14
nn = 5
u_hub_lowerbound = 5.777777777777778
c_pow = 0.59230249
p_max = 0.5 * rho * area * c_pow * free**3
# Initialize Model
m = GEKKO(remote=True)
#initialize variables, Set lower and upper bounds
x = [m.Var(value = 0.03902278, lb = 0, ub = axi_max) \
for i in range(nn)]
# i = 0
b = 1
c = 0
v_s = list()
for i in range(nn-1): # Loop runs for nn-1 times
# print(i)
# print(i,b,c)
squared_defs = list()
while i < b:
d = distances[b][c][0]
r = distances[b][c][1]
ss = (2 * (alpha * d) / diam)
tt = r / ((diam/2) + (alpha * d))
squared_defs.append((2 * x[i] / (1 + ss**2)) * np.exp(-(tt**2)) ** 2)
i+=1
c+=1
#Equations
m.Equation((free * (1 - (sum(squared_defs))**0.5)) - rat <= 0)
m.Equation((free * (1 - (sum(squared_defs))**0.5)) - u_hub_lowerbound >= 0)
v_s.append(free * (1 - (sum(squared_defs))**0.5))
squared_defs.clear()
b+=1
c=0
# Inserts free as the first item on the v_s list to
# increase len(v_s) to nn, so that 'v_s' and 'x'
# are of same length
v_s.insert(0, free)
gamma = list()
for i in range(len(x)):
bet = (4*x[i]*((1-x[i])**2) * rho * area) / 2
gam = bet * v_s[i]**3
gamma.append(gam)
#Equations
m.Equation(x[i] - axi_max <= 0)
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) \
* v_s[i]**3) - p_max <= 0)
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) * \
v_s[i]**3) > 0)
#Equation
m.Equation(col_total - sum(gamma) <= 0)
#Objective
y = sum(gamma)
m.Maximize(y) # Maximize
#Set global options
m.options.IMODE = 3 #steady state optimization
#Solve simulation
m.options.SOLVER = 3
m.solver_options = ['linear_solver ma27','mu_strategy adaptive','max_iter 2500', 'tol 1.0e-5' ]
m.solve()
Built the equations without .value in the expressions. The x[i].value is only needed at the end to view the solution after the solution is complete or to initialize the value of x[i]. The expression m.Maximize(y) is more readable than m.Obj(-y) although they are equivalent.
from gekko import GEKKO
import numpy as np
distances = np.array([[[0, 0],[0,0],[0,0],[0,0]],\
[[155,0],[0,0],[0,0],[0,0]],\
[[310,0],[155,0],[0,0],[0,0]],\
[[465,0],[310,0],[155,0],[0,0]],\
[[620,0],[465,0],[310,0],[155,0]]])
alpha = 0.5 / np.log(30/0.075)
diam = 31
free = 7
rho = 1.2253
area = np.pi * (diam / 2)**2
min_v = 5.5
axi_max = 0.069262150781
col_total = 20000
p_max = 4000
rat = 14
nn = 5
# Initialize Model
m = GEKKO(remote=True)
#initialize variables, Set lower and upper bounds
x = [m.Var(value = 0.03902278, lb = 0, ub = axi_max) \
for i in range(nn)]
i = 0
b = 1
c = 0
v_s = list()
for turbs in range(nn-1): # Loop runs for nn-1 times
squared_defs = list()
while i < b:
d = distances[b][c][0]
r = distances[b][c][1]
ss = (2 * (alpha * d) / diam)
tt = r / ((diam/2) + (alpha * d))
squared_defs.append((2 * x[i] / (1 + ss**2)) \
* m.exp(-(tt**2)) ** 2)
i+=1
c+=1
#Equations
m.Equation((free * (1 - (sum(squared_defs))**0.5)) - rat <= 0)
m.Equation(min_v - (free * (1 - (sum(squared_defs))**0.5)) <= 0 )
v_s.append(free * (1 - (sum(squared_defs))**0.5))
squared_defs.clear()
b+=1
a=0
c=0
# Inserts free as the first item on the v_s list to
# increase len(v_s) to nn, so that 'v_s' and 'x'
# are of same length
v_s.insert(0, free)
beta = list()
gamma = list()
for i in range(len(x)):
bet = (4*x[i]*((1-x[i])**2) * rho * area) / 2
gam = bet * v_s[i]**3
#Equations
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) \
* v_s[i]**3) - p_max <= 0)
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) \
* v_s[i]**3) > 0)
gamma.append(gam)
#Equation
m.Equation(col_total - sum(gamma) <= 0)
#Objective
y = sum(gamma)
m.Maximize(y) # Maximize
#Set global options
m.options.IMODE = 3 #steady state optimization
#Solve simulation
m.options.SOLVER = 3
m.solve()
This gives a successful solution with maximized objective 20,000:
Number of Iterations....: 12
(scaled) (unscaled)
Objective...............: -4.7394814741924645e+00 -1.9999999999929641e+04
Dual infeasibility......: 4.4698510326511536e-07 1.8862194343304290e-03
Constraint violation....: 3.8275766582203308e-11 1.2941979026166479e-07
Complementarity.........: 2.1543608536533588e-09 9.0911246952931704e-06
Overall NLP error.......: 4.6245685940749926e-10 1.8862194343304290e-03
Number of objective function evaluations = 80
Number of objective gradient evaluations = 13
Number of equality constraint evaluations = 80
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 13
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 12
Total CPU secs in IPOPT (w/o function evaluations) = 0.010
Total CPU secs in NLP function evaluations = 0.011
EXIT: Optimal Solution Found.
The solution was found.
The final value of the objective function is -19999.9999999296
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 3.210000000399305E-002 sec
Objective : -19999.9999999296
Successful solution
---------------------------------------------------
I need help with a dynamic optimization problem that consist in a consumed energy optimization of a UAV with this optimal control problem.
enter image description here
enter image description here
My code is this
Ecuations:
Parameters
tf
#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000
t1 = 0, >=0
t2 = 0, >=0
t3 = 0, >=0
t4 = 0, >=0
Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359
#Motor Coefficients
J = 4.1904e-5 !kg*m^2
kt = 0.0104e-3 !N*m/A
kv = 96.342 !rad/s/volt
Dv = 0.2e-3 !N*m*s/rad
R = 0.2 !Ohms
#Battery parameters
Q = 1.55 !Ah
Rint = 0.02 !Ohms
E0 = 1.24 !volt
K = 2.92e-3 !volt
A = 0.156
B =2.35
#Quadrotor parameters
l = 0.175 !m
m = 1.3 !kg
Ix = 0.081 !kg*m^2
Iy = 0.081 !kg*m^2
Iz = 0.142 !kg*m^2
kb = 3.8305e-6 !N/rad/s
ktau = 2.2518e-8 !(N*m)/rad/s
#Parametrizacion del polinomio
a1 = -1.72e-5
a2 = 1.95e-5
a3 = -6.98e-6
a4 = 4.09e-7
b1 = 0.014
b2 = -0.0157
b3 = 5.656e-3
b4 = -3.908e-4
c1 = -0.8796
c2 = 0.3385
c3 = 0.2890
c4 = 0.1626
Variables
!------------------CONDICONES INICIALES------------------!
x = 0
xp = 0
y = 0
yp = 0
z = 0
zp = 0
pitch = 0, >=-pi/2, <=pi/2 !theta - restricciones
pitchp = 0
roll = 0, >=-pi/2, <=pi/2 !phi - restricciones
rollp = 0
yaw = 0 !psi
yawp = 0%, >=-200/180, <=200/180
#Función objetivo
of = 0 !condición inicial de la función objetivo
Intermediates
#Motor 1
aw1 = a1*w1^2 + b1*w1 + c1
bw1 = a2*w1^2 + b2*w1 + c2
cw1 = a3*w1^2 + b3*w1 + c3
dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
aw2 = a1*w2^2 + b1*w2 + c1
bw2 = a2*w2^2 + b2*w2 + c2
cw2 = a3*w2^2 + b3*w2 + c3
dw2 = a4*w2^2 + b4*w2 + c4
#Motor 3
aw3 = a1*w3^2 + b1*w3 + c1
bw3 = a2*w3^2 + b2*w3 + c2
cw3 = a3*w3^2 + b3*w3 + c3
dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
aw4 = a1*w4^2 + b1*w4 + c1
bw4 = a2*w4^2 + b2*w4 + c2
cw4 = a3*w4^2 + b3*w4 + c3
dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
u1 = kb * (w2^2 - w4^2)
u2 = kb * (w3^2 - w1^2)
u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
Ectotal = Ec1 + Ec2 + Ec3 + Ec4
Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
xp = $x
yp = $y
zp = $z
pitchp = $pitch
rollp = $roll
yawp = $yaw
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
tf * x = 4
tf * y = 5
tf * z = 6
tf * xp = 0
tf * yp = 0
tf * zp = 0
tf * roll = 0
tf * pitch = 0
tf * yaw = 0
!-----------------TORQUE DE LOS MOTORES------------------!
t1 = J*$w1 + ktau*w1^2 + Dv*w1
t2 = J*$w2 + ktau*w2^2 + Dv*w2
t3 = J*$w3 + ktau*w3^2 + Dv*w3
t4 = J*$w4 + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
m*$zp = (cos(roll)*cos(pitch))*T-m*g
Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
$of = Ectotal
MATLAB:
clear all; close all; clc
server = 'http://127.0.0.1';
app = 'traj_optima';
addpath('C:/Program Files/MATLAB/apm_matlab_v0.7.2/apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');
apm_option(server,app,'apm.max_iter',200);
apm_option(server,app,'nlc.nodes',3);
apm_option(server,app,'apm.rtol',1);
apm_option(server,app,'apm.otol',1);
apm_option(server,app,'nlc.solver',3);
apm_option(server,app,'nlc.imode',6);
apm_option(server,app,'nlc.mv_type',1);
costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1');
apm_option(server,app,'w1.status',1);
apm_info(server,app,'MV','w2');
apm_option(server,app,'w2.status',1);
apm_info(server,app,'MV','w3');
apm_option(server,app,'w3.status',1);
apm_info(server,app,'MV','w4');
apm_option(server,app,'w4.status',1);
% Torques
apm_info(server,app,'MV','t1');
apm_option(server,app,'t1.status',1);
apm_info(server,app,'MV','t2');
apm_option(server,app,'t2.status',1);
apm_info(server,app,'MV','t3');
apm_option(server,app,'t3.status',1);
apm_info(server,app,'MV','t4');
apm_option(server,app,'t4.status',1);
%Salida
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app);
z = y.x;
tiempo2.csv
time,tf
0,0
0.001,0
0.2,0
0.4,0
0.6,0
0.8,0
1,0
1.2,0
1.4,0
1.6,0
1.8,0
2,0
2.2,0
2.4,0
2.6,0
2.8,0
3,0
3.2,0
3.4,0
3.6,0
3.8,0
4,0
4.2,0
4.4,0
4.6,0
4.8,0
5,0
5.2,0
5.4,0
5.6,0
5.8,0
6,0
6.2,0
6.4,0
6.6,0
6.8,0
7,0
7.2,0
7.4,0
7.6,0
7.8,0
8,0
8.2,0
8.4,0
8.6,0
8.8,0
9,0
9.2,0
9.4,0
9.6,0
9.8,0
10,1
Finally the answer obtained is:
enter image description here
I need help with this local infeasibility problem, please.
The infeasible solution is caused by the terminal constraints:
tf * z = 4
tf * z = 5
tf * z = 6
When tf=0, the constraints are evaluated to 0=4, 0=5, 0=6 and the solver reports that these can not be satisfied by the solver. Instead, you can pose the constraints as:
tf * (x-4) = 0
tf * (y-5) = 0
tf * (z-6) = 0
That way, the constraint is valid when tf=0 and when tf=1 at the final time. A potential better way yet is to convert the terminal constraints to objective terms with f=1000 such as:
minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
minimize f*tf*(xp^2 + yp^2 + zp^2)
minimize f*tf*(roll^2 + pitch^2 + yaw^2)
That way, the optimizer won't report an infeasible solution if it can't reach the terminal constraints as discussed in the pendulum problem. I made a few other modifications to your model and script to achieve a successful solution. Here is a summary:
Converted terminal constraints to objective function (soft constraints)
Parameters t1-t4 should be variables
Fixed degree of freedom issue by making w1-w4 variables and w1p-w4p variables. w1-w4 are differential states.
Added constraints to w1p-w4p between -10 and 10 to help the solver converge
Added initialization step to simulate the model before optimizing. There are more details on initialization strategies in this paper: Safdarnejad, S.M., Hedengren, J.D., Lewis, N.R., Haseltine, E., Initialization Strategies for Optimization of Dynamic Systems, Computers and Chemical Engineering, 2015, Vol. 78, pp. 39-50, DOI: 10.1016/j.compchemeng.2015.04.016
Model
Parameters
tf
w1p = 0 > -10 < 10
w2p = 0 > -10 < 10
w3p = 0 > -10 < 10
w4p = 0 > -10 < 10
Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359
#Motor Coefficients
J = 4.1904e-5 !kg*m^2
kt = 0.0104e-3 !N*m/A
kv = 96.342 !rad/s/volt
Dv = 0.2e-3 !N*m*s/rad
R = 0.2 !Ohms
#Battery parameters
Q = 1.55 !Ah
Rint = 0.02 !Ohms
E0 = 1.24 !volt
K = 2.92e-3 !volt
A = 0.156
B =2.35
#Quadrotor parameters
l = 0.175 !m
m = 1.3 !kg
Ix = 0.081 !kg*m^2
Iy = 0.081 !kg*m^2
Iz = 0.142 !kg*m^2
kb = 3.8305e-6 !N/rad/s
ktau = 2.2518e-8 !(N*m)/rad/s
#Parametrizacion del polinomio
a1 = -1.72e-5
a2 = 1.95e-5
a3 = -6.98e-6
a4 = 4.09e-7
b1 = 0.014
b2 = -0.0157
b3 = 5.656e-3
b4 = -3.908e-4
c1 = -0.8796
c2 = 0.3385
c3 = 0.2890
c4 = 0.1626
Variables
!------------------CONDICONES INICIALES------------------!
x = 0
xp = 0
y = 0
yp = 0
z = 0
zp = 0
pitch = 0, >=-pi/2, <=pi/2 !theta - restricciones
pitchp = 0
roll = 0, >=-pi/2, <=pi/2 !phi - restricciones
rollp = 0
yaw = 0 !psi
yawp = 0 %, >=-200/180, <=200/180
#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000
t1 = 0, >=0
t2 = 0, >=0
t3 = 0, >=0
t4 = 0, >=0
#Función objetivo
of = 0 !condición inicial de la función objetivo
Intermediates
#Motor 1
aw1 = a1*w1^2 + b1*w1 + c1
bw1 = a2*w1^2 + b2*w1 + c2
cw1 = a3*w1^2 + b3*w1 + c3
dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
aw2 = a1*w2^2 + b1*w2 + c1
bw2 = a2*w2^2 + b2*w2 + c2
cw2 = a3*w2^2 + b3*w2 + c3
dw2 = a4*w2^2 + b4*w2 + c4
#Motor 3
aw3 = a1*w3^2 + b1*w3 + c1
bw3 = a2*w3^2 + b2*w3 + c2
cw3 = a3*w3^2 + b3*w3 + c3
dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
aw4 = a1*w4^2 + b1*w4 + c1
bw4 = a2*w4^2 + b2*w4 + c2
cw4 = a3*w4^2 + b3*w4 + c3
dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
u1 = kb * (w2^2 - w4^2)
u2 = kb * (w3^2 - w1^2)
u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
Ectotal = Ec1 + Ec2 + Ec3 + Ec4
! scaling factor for terminal constraint
f = 1000
Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
xp = $x
yp = $y
zp = $z
pitchp = $pitch
rollp = $roll
yawp = $yaw
w1p = $w1
w2p = $w2
w3p = $w3
w4p = $w4
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
#tf * (x-4) = 0
#tf * (y-5) = 0
#tf * (z-6) = 0
#tf * xp = 0
#tf * yp = 0
#tf * zp = 0
#tf * roll = 0
#tf * pitch = 0
#tf * yaw = 0
minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
minimize f*tf*(xp^2 + yp^2 + zp^2)
minimize f*tf*(roll^2 + pitch^2 + yaw^2)
!-----------------TORQUE DE LOS MOTORES------------------!
t1 = J*w1p + ktau*w1^2 + Dv*w1
t2 = J*w2p + ktau*w2^2 + Dv*w2
t3 = J*w3p + ktau*w3^2 + Dv*w3
t4 = J*w4p + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
m*$zp = (cos(roll)*cos(pitch))*T-m*g
Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
$of = Ectotal
MATLAB Script
clear all; close all; clc
server = 'http://byu.apmonitor.com';
app = 'traj_optima';
addpath('apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');
apm_option(server,app,'apm.max_iter',1000);
apm_option(server,app,'apm.nodes',3);
apm_option(server,app,'apm.rtol',1e-6);
apm_option(server,app,'apm.otol',1e-6);
apm_option(server,app,'apm.solver',3);
apm_option(server,app,'apm.imode',6);
apm_option(server,app,'apm.mv_type',1);
costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1p');
apm_option(server,app,'w1p.status',1);
apm_info(server,app,'MV','w2p');
apm_option(server,app,'w2p.status',1);
apm_info(server,app,'MV','w3p');
apm_option(server,app,'w3p.status',1);
apm_info(server,app,'MV','w4p');
apm_option(server,app,'w4p.status',1);
%Salida
disp('')
disp('------------- Initialize ----------------')
apm_option(server,app,'apm.coldstart',1);
output = apm(server,app,'solve');
disp(output)
disp('')
disp('-------------- Optimize -----------------')
apm_option(server,app,'apm.time_shift',0);
apm_option(server,app,'apm.coldstart',0);
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app);
z = y.x;
This gives a successful solution but the terminal constraints are not met. The solver optimizes the use of w1p-w4p to minimize the objective but there is no solution that makes it to the terminal constraints.
The solution was found.
The final value of the objective function is 50477.4537378181
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 3.06940000000759 sec
Objective : 50477.4537378181
Successful solution
---------------------------------------------------
As a next step, I recommend that you increase the number of time points or allow the final time to change to meet the terminal constraints. You may also want to consider switching to Python Gekko that uses the same underlying engine as APM MATLAB. In this case, the modeling language is fully integrated with Python.