m.Equations resulting in TypeError: 'int' object is not subscriptable

Question

I'm having trouble passing my equations of motion to the solver on my control optimization problem.

Just a little explanation on what I'm attempting to do here, because I think there are two problem areas:

First, I'm defining a contact switch c that is used to turn on and off portions of the dynamic equations based on the value a, which is a FV between 0 and .45. I have a loop which sets the value of c[i] based on the value of the time parameter relative to a.

c = [None]*N

for i in range(N):
    difference = m.Intermediate(.5-m.time[i])
    abs = m.if3(difference, -difference, difference)
    c[i] = m.Intermediate(m.if3(abs-(.5-a), 1, 0))

It should resemble a vector of length N:

c= [0, 0, 0, 1, 1, ...., 1, 1, 0, 0, 0] 

It's not clear if this was implemented properly, but it's not throwing me errors at this point. (Note: I'm aware that this can be easily implemented as a mixed-integer variable, but I really want to use IPOPT, so I'm using the m.if3() method to create the binary switch.)

Second, I'm getting an error when passing the equations of motion. This exists whether the c is included, so, at least for right now, I know that is not the issue.

m.Equations(xdot.dt()/TF == c*u*(L1*m.sin(q1)-L2*m.sin(q1+q2))/(M*L1*L2*m.sin(2*q1+q2)))
m.Equations(ydot.dt()/TF == -c*u*(L1*m.cos(q1)+L2*m.cos(q1+q2))/(M*L1*L2*m.sin(2*q1+q2))-g/m)
m.Equation(x.dt()/TF == xdot)
m.Equation(y.dt()/TF == ydot)
m.Equation(y*init == y*final) #initial and final y position must be equal

TypeError: 'int' object is not subscriptable

I've attempted to set up an intermediate loop to handle the RH of the equation to no avail:

RH = [None]*N
RH = m.Intermediate([c[i]*u[i]*(L1*m.sin(q1[i])-2*m.sin(q1[i]+q2[i]))/(M*L1*L2*m.sin(2*q1[i]+q2[i])) for i in range(N)])
m.Equations(xdot.dt()/TF == RH)

Below is the full code. Note: there are probably other issues both in my code and problem definition, but I'm just looking to find a way to successfully pass these equations of motion. Much appreciated!

Full code:

import math
import numpy as np
from gekko import GEKKO

#Defining a model
m = GEKKO(remote=True)
v = 1  #set walking speed (m/s)
L1 = .5 #set thigh length (m)
L2 = .5 #set shank length (m)
M = 75 #set mass (kg)

#################################
#Define secondary parameters
D = L1 + L2  #leg length parameter
pi = math.pi #define pi
g = 9.81 #define gravity

#Define initial and final conditions and limits
x0 = -v/2; xf = v/2
xdot0 = v; xdotf = v
ydot0 = 0; ydotf = 0
ymin = .5*D; ymax = 1.5*D
q1min = -pi/2; q1max = pi/2
q2min = -pi/2; q2max = 0
tfmin = D/(2*v); tfmax = 3*D/(2*v)

#Defining the time parameter (0, 1)
N = 100
t = np.linspace(0,1,N)
m.time = t

#Final time Fixed Variable
TF = m.FV(1,lb=tfmin,ub=tfmax); TF.STATUS = 1
end_loc = len(m.time)-1
amin = 0; amax = .45

#Defining initial and final condition vectors
init = np.zeros(len(m.time))
final = np.zeros(len(m.time))
init[1] = 1
final[-1] = 1

init = m.Param(value=init)
final = m.Param(value=final)

#Parameters
M = m.Param(value=M) #cart mass
L1 = m.Param(value=L1) #link 1 length
L2 = m.Param(value=L2) #link 1 length
g = m.Const(value=g) #gravity

#Control Input Manipulated Variable
u = m.MV(0); u.STATUS = 1

#Ground Contact Fixed Variable
a = m.FV(0,lb=amin,ub=amax)  #equates to the unscaled time when contact first occurs

#State Variables
x, y, xdot, ydot, q1, q2 = m.Array(m.Var, 6)

x.value = x0;
xdot.value = xdot0; ydot.value = ydot0
y.LOWER = ymin; y.UPPER = ymax
q1.LOWER = q1min; q1.UPPER = q1max
q2.LOWER = q2min; q2.UPPER = q2max

#Intermediates
c = [None]*N

for i in range(N):
    difference = m.Intermediate(.5-m.time[i])
    abs = m.if3(difference, -difference, difference)
    c[i] = m.Intermediate(m.if3(abs-(.5-a), 1, 0))

#Defining the State Space Model
m.Equations(xdot.dt()/TF == c*u*(L1*m.sin(q1)-L2*m.sin(q1+q2))/(M*L1*L2*m.sin(2*q1+q2))) ####This produces the error
m.Equations(ydot.dt()/TF == -c*u*(L1*m.cos(q1)+L2*m.cos(q1+q2))/(M*L1*L2*m.sin(2*q1+q2))-g/m)
m.Equation(x.dt()/TF == xdot)
m.Equation(y.dt()/TF == ydot)
m.Equation(y*init == y*final) #initial and final y position must be equal

#Defining final condition
m.fix_final(x,val=xf)
m.fix_final(xdot,val=xdotf)
m.fix_final(xdot,val=ydotf)

#Try to minimize final time and torque
m.Obj(TF)
m.Obj(0.001*u**2)

m.options.IMODE = 6 #MPC
m.options.SOLVER = 3
m.solve()

m.time = np.multiply(TF, m.time)

Answer 1

Nice application. Here are a few corrections and ideas:

1. Use a switch condition that uses a NumPy array. There is no need to define the individual points in the horizon with c[i].

#Intermediates
#c = [None]*N

#for i in range(N):
#    difference = m.Intermediate(.5-m.time[i])
#    abs = m.if3(difference, -difference, difference)
#    c[i] = m.Intermediate(m.if3(abs-(.5-a), 1, 0))
diff = 0.5 - m.time
adiff = m.Param(np.abs(diff))
swtch = m.Intermediate(adiff-(0.5-a))
c = m.if3(swtch,1,0)

You may be able to use the m.integral() function to set the value of c to 1 and keep it there when contact is made.

2. Use the m.periodic(y) function to set the initial value of y equal to the final value of y.

#m.Equation(y*init == y*final) #initial and final y position must be equal
m.periodic(y)

3. Try using soft constraints instead of hard constraints if there is a problem with finding a feasible solution.

#Defining final condition
#m.fix_final(x,val=xf)
#m.fix_final(xdot,val=xdotf)
#m.fix_final(ydot,val=ydotf)
m.Minimize(final*(x-xf)**2)
m.Minimize(final*(xdot-xdotf)**2)
m.Minimize(final*(ydot-ydotf)**2)

4. The m.if3() function requires the APOPT solver. Try m.if2() for the continuous version that uses MPCCs instead of binary variables to define the switch. The integral function may be alternative way to avoid a binary variable.

Here is the final code that attempts a solution, but the solver can't yet find a solution. I hope this helps you get a little further on your optimization problem. You may need to use a shooting (sequential method) to find an initial feasible solution.

import math
import numpy as np
from gekko import GEKKO

#Defining a model
m = GEKKO(remote=True)
v = 1  #set walking speed (m/s)
L1 = .5 #set thigh length (m)
L2 = .5 #set shank length (m)
M = 75 #set mass (kg)

#################################
#Define secondary parameters
D = L1 + L2  #leg length parameter
pi = math.pi #define pi
g = 9.81 #define gravity

#Define initial and final conditions and limits
x0 = -v/2; xf = v/2
xdot0 = v; xdotf = v
ydot0 = 0; ydotf = 0
ymin = .5*D; ymax = 1.5*D
q1min = -pi/2; q1max = pi/2
q2min = -pi/2; q2max = 0
tfmin = D/(2*v); tfmax = 3*D/(2*v)

#Defining the time parameter (0, 1)
N = 100
t = np.linspace(0,1,N)
m.time = t

#Final time Fixed Variable
TF = m.FV(1,lb=tfmin,ub=tfmax); TF.STATUS = 1
end_loc = len(m.time)-1
amin = 0; amax = .45

#Defining initial and final condition vectors
init = np.zeros(len(m.time))
final = np.zeros(len(m.time))
init[1] = 1
final[-1] = 1

init = m.Param(value=init)
final = m.Param(value=final)

#Parameters
M = m.Param(value=M) #cart mass
L1 = m.Param(value=L1) #link 1 length
L2 = m.Param(value=L2) #link 1 length
g = m.Const(value=g) #gravity

#Control Input Manipulated Variable
u = m.MV(0); u.STATUS = 1

#Ground Contact Fixed Variable
a = m.FV(0,lb=amin,ub=amax)  #equates to the unscaled time when contact first occurs

#State Variables
x, y, xdot, ydot, q1, q2 = m.Array(m.Var, 6)

x.value = x0;
xdot.value = xdot0; ydot.value = ydot0
y.LOWER = ymin; y.UPPER = ymax
q1.LOWER = q1min; q1.UPPER = q1max
q2.LOWER = q2min; q2.UPPER = q2max

#Intermediates
#c = [None]*N

#for i in range(N):
#    difference = m.Intermediate(.5-m.time[i])
#    abs = m.if3(difference, -difference, difference)
#    c[i] = m.Intermediate(m.if3(abs-(.5-a), 1, 0))
diff = 0.5 - m.time
adiff = m.Param(np.abs(diff))
swtch = m.Intermediate(adiff-(0.5-a))
c = m.if3(swtch,1,0)

#Defining the State Space Model
m.Equation(xdot.dt()/TF == c*u*(L1*m.sin(q1)
                                -L2*m.sin(q1+q2))
                           /(M*L1*L2*m.sin(2*q1+q2)))
m.Equation(ydot.dt()/TF == -c*u*(L1*m.cos(q1)
                                  +L2*m.cos(q1+q2))
                            /(M*L1*L2*m.sin(2*q1+q2))-g/M)
m.Equation(x.dt()/TF == xdot)
m.Equation(y.dt()/TF == ydot)
#m.Equation(y*init == y*final) #initial and final y position must be equal
m.periodic(y)

#Defining final condition
#m.fix_final(x,val=xf)
#m.fix_final(xdot,val=xdotf)
#m.fix_final(ydot,val=ydotf)
m.Minimize(final*(x-xf)**2)
m.Minimize(final*(xdot-xdotf)**2)
m.Minimize(final*(ydot-ydotf)**2)

#Try to minimize final time and torque
m.Minimize(TF)
m.Minimize(0.001*u**2)

m.options.IMODE = 6 #MPC
m.options.SOLVER = 1
m.solve()

m.time = np.multiply(TF, m.time)