Alternatives to matrix inversions in GEKKO model - gekko

Question

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])

Answer 1

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