How to properly apply equality boundary constraint

Question

I am doing optimization of an inverted pendulum using Python SCIPY minimize. Currently, I am having trouble figuring out how to implement boundary constraints shown here: Boundary Constraints

Here is my code:

import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize



#parameters
m1 = 1
m2 = 0.3
g = 9.81
l = 0.5
dmax = 2.0
umax = 20.0
T = 2
d = 1
h = 0.2

# (x0,u0) (x1,u1) ....
# +-------+--------+-------+------+--------+------> 1
# y = [x0, u0, x1, u1, ..., x5, u5]
# Alt.:
# y = [x0, x1, x2, ..., u0, ... u5]
# x = y[:24]
# u = y[-6:] = y[24:]

#Objective 
def objective(x):      
    return np.sum(x[-6:]**2)


#EOM's SS
# dx_i = statespace(xi[0], xi[1], xi[2], xi[3], ui)
def statespace(y1,y2,ydot1,ydot2,u):    # Should only provide the 4 states; the others are fixed paramters.

    dy1 = ydot1
    dy2 = ydot2
    dydot1 = ((l*m2*np.sin(y1)*y1*y1) + u + (m2*g*np.cos(y1)*np.sin(y1))) / (m1 + m2*(1-np.cos(y1)**2))
    dydot2 = -1*((l*m2*np.cos(y2)*np.sin(y2)*y2*y2) + u*np.cos(y2) + ((m1+m2)*g*np.sin(y2))) / (l*m1 + l*m2*(1-np.cos(y2)**2))
    return np.array([dy1,dy2,dydot1,dydot2])      


#trap(y) == 0
def trap(y,f=statespace):
    x = y[:24].reshape(6,4)
    u = y[24:]
    c = np.zeros((5,4))
    for k in range(5):
         f1 = f(x[k+1,0], x[k+1,1], x[k+1,2], x[k+1,3], u[k+1])
         f0 = f(x[k,0],x[k,1],x[k,2],x[k,3],u[k])
         c[k] = x[k+1]-x[k]-(h/2)*(f1+f0)
         return c.reshape(-1)



#Initial Guess
#steps = 
#x0 = [# of states + # of controls]x[# of steps] * steps  
x0 = np.zeros(30)


# End points:

# Starting point:
def constraint_start(y):
    x0 = y[:4]
    return x0

#End point
def constraint_end(y):
    xf = y[:24]
    return xf[d]

#bounds
b = (-dmax,dmax)
c = (-umax,umax)  
bnds = [(b), (-np.inf, np.inf), (-np.inf, np.inf),(-np.inf, np.inf),(b),(-np.inf, np.inf),(-np.inf, np.inf),(-np.inf, np.inf),(b),(-np.inf, np.inf), 
(-np.inf, np.inf), (-np.inf, np.inf),(b),(-np.inf, np.inf),(-np.inf, np.inf),(-np.inf, np.inf),(b),(-np.inf, np.inf),(-np.inf, np.inf),(-np.inf, np.inf),
(b),(-np.inf, np.inf),(-np.inf, np.inf),(-np.inf, np.inf),(c),(c),(c),(c),(c),(c)]

con1 = {'type': 'eq', 'fun': constraint_start}
con2 = {'type': 'eq', 'fun': constraint_end}
con3 = {'type': 'eq', 'fun': trap}
cons = (con1,con2,con3)                  

sol = minimize(objective, x0, bounds = bnds, constraints = cons) 
print(sol)

And here is the specific parts I am stuck with. I have tried to implement something. I am confident in "constraint_start" but not "constraint_end", since the values are not all 0 and are supposed to be applied to my last 4 states. (Note: my collocation points are organized to be 24 state variables and 6 control variables in that order). The very beginning of the code shows this in comments.

# Starting point:
def constraint_start(y):
    x0 = y[:4]
    return x0

#End point
def constraint_end(y):
    xf = y[:24]
    return xf[d]

score 0 · Accepted Answer · answered Jun 28 '21 at 01:51

I don't see many (any?) examples of constraints, either in the docs or in SO questions. But here's a successful attempt at applying a equality constraint between two elements of an array.

First a simple single variable minimization:

In [50]: minimize(lambda x: x**2+x+1, [0])
Out[50]: 
      fun: 0.75
 hess_inv: array([[1]])
      jac: array([2.23517418e-08])
  message: 'Optimization terminated successfully.'
     nfev: 6
      nit: 1
     njev: 3
   status: 0
  success: True
        x: array([-0.5])

Now the same thing, but written with a (2,) variable, and the constraint that x[1] == x[0]+1

In [51]: minimize(lambda x: x[0]**2+x[1], [0,0],
   constraints=[{'type':'eq','fun':lambda x:x[0]-x[1]+1}])
Out[51]: 
     fun: 0.75
     jac: array([-1.,  1.])
 message: 'Optimization terminated successfully'
    nfev: 16
     nit: 5
    njev: 5
  status: 0
 success: True
       x: array([-0.50000001,  0.49999999])

The constraint fun returns 0 if the relation between the x elements is met.

Thank you very much for your answer. I am okay with examples such as that but have been confused with mine. Basically, I need to say that my last 4 values are restricted to (0,d) and (0,pi). Therefore, would something like this make sense? `def constraint_end(y): y = [(0,d),[0,np.pi]] xf = y[20:24]` — jbiscan, Jun 28 '21 at 13:19
What's `xf` supposed to be? In my example, the constraint function returns a 0 (scalar) if (some combination of) the values meet a criteria. Non zero if they don't. My advise is to start small with a working an example, and work your way gradually toward something bigger. You need to understand, and verify, each increment in complexity. — hpaulj, Jun 28 '21 at 15:20

How to properly apply equality boundary constraint

1 Answers1