Three Tank multimode problem (python/casadi)

Casadi

The model in Python code for a fixed control discretization grid using direct multiple shooting. We use pycombina for solving the (CIA) problem.

# Three tank optimal control
# ----------------------
# An optimal control problem (OCP),
# solved with direct multiple-shooting.
#
# For more information on CasADi see: http://labs.casadi.org/OCP
from casadi import *
import pylab as pl
from pycombina import BinApprox, CombinaBnB
import matplotlib.pyplot as plt

############# Input variables ############# 
T = 12
N = 100 # number of control intervals


############# End of Input variables ############# 

opti = Opti() # Optimization problem


#------- parameter values --------------
# Model parameters
k_1 = 2
k_2 = 3
k_3 = 1
k_4 = 3


c_1 = 1
c_2 = 2
c_3 = 0.8
T = 12

# ---- decision variables ---------
X = opti.variable(4,N+1) # state trajectory
x1   = X[0,:]
x2 = X[1,:]
x3 = X[2,:]
x4 = X[3,:]


U = opti.variable(3,N)   # control trajectory (throttle)
u1   = U[0,:]
u2 = U[1,:]
u3 = U[2,:]

# ---- objective          ---------
opti.minimize(x4[N]) # Objective as Mayer term

# ---- dynamic constraints -------
f = lambda x,u: vertcat(c_1*u[0]+c_2*u[1]-(x[0])**0.5-  u[2]* (c_3*x[0])**0.5 , (x[0])**0.5- (x[1])**0.5, (x[1])**0.5- (x[2])**0.5 +u[2]* (c_3*x[0])**0.5 , u[1]*0.0+ k_1*(x[1]-k_2)**2 + k_3*(x[2]-k_4)**2 ) # dx/dt = f(x,u)


dt = T/N # length of a control interval
for k in range(N): # loop over control intervals
   # Runge-Kutta 4 integration
   k1 = f(X[:,k],         U[:,k])
   k2 = f(X[:,k]+dt/2*k1, U[:,k])
   k3 = f(X[:,k]+dt/2*k2, U[:,k])
   k4 = f(X[:,k]+dt*k3,   U[:,k])
   x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4) 
   opti.subject_to(X[:,k+1]==x_next) # close the gaps

# ---- path constraints -----------

opti.subject_to(opti.bounded(0,U,1)) # control is limited
opti.subject_to(u1+u2+u3 == 1) # SOS1 constraint


# ---- boundary conditions --------
opti.subject_to(x1[0]==2)   # intitial values
opti.subject_to(x2[0]==2) # ... 
opti.subject_to(x3[0]==2) # ... 
opti.subject_to(x4[0]==0) # ... mayer term auxiliary state

#opti.subject_to(pos[-1]==1)  # finish line at position 1

# ---- misc. constraints  ----------
#opti.subject_to(T>=0) # Time must be positive

# ---- initial values for solver ---
opti.set_initial(x1, 2)
opti.set_initial(x2, 2)
opti.set_initial(x3, 2)
opti.set_initial(x4, 0)
opti.set_initial(u1, 1)
opti.set_initial(u2, 0)
opti.set_initial(u3, 0)


# ---- solve NLP              ------
opti.solver("ipopt") # set numerical backend
sol = opti.solve()   # actual solve


# ---- post-processing        ------
from pylab import plot, step, figure, legend, show, spy

plt.plot(pl.linspace(0, 12, num=N+1),sol.value(x1),label="x1")
plt.plot(pl.linspace(0, 12, num=N+1),sol.value(x2),label="x2")
plt.plot(pl.linspace(0, 12, num=N+1),sol.value(x3),label="x3")

#plot(limit(sol.value(pos)),'r--',label="speed limit")
plt.step(pl.linspace(0, 12, num=N),sol.value(U[0,:]),'k',label="a1")
plt.step(pl.linspace(0, 12, num=N),sol.value(U[1,:]),label="a2")
plt.step(pl.linspace(0, 12, num=N),sol.value(U[2,:]),label="a3")

plt.xlabel('t')
plt.ylabel('state and control values')
plt.title('Differential state trajectories for relaxed controls')

#xlabel=("t")
plt.legend(loc="upper left")
#title="
plt.savefig('three_tank_relaxed_solution.png')
  
#################################################

### Setting up CIA problem

 ################################################

t = np.linspace(0,T,N+1)
b_rel = np.array([[min(1,max(0,x)) for x in sol.value(U[0,:])], [min(1,max(0,x)) for x in sol.value(U[1,:])], [min(1,max(0,x)) for x in sol.value(U[2,:])]])


binapprox = BinApprox(t = t, b_rel = b_rel, binary_threshold = 1e-3, \
    off_state_included = False)

#binapprox.set_n_max_switches(n_max_switches = max_switches)
#binapprox.set_min_up_times(min_up_times = [min_up_time])
#binapprox.set_min_down_times(min_down_times = [min_down_time])

combina = CombinaBnB(binapprox)
combina.solve(use_warm_start=False, bnb_search_strategy='dfs', bnb_opts={'max_iter': 1000000})

b_bin_orig = pl.asarray(binapprox.b_bin)


#################################################

### Re-run Optimization Problem with fixed binary controls
### 'Dirty hack': just copy previous problem set up due to reuse issues of constraints

sol1_x1 = sol.value(x1)
sol1_x2 = sol.value(x2)
sol1_x3 = sol.value(x3)


opti2 = Opti() # Optimization problem

# ---- decision variables ---------
X = opti2.variable(4,N+1) # state trajectory
x1   = X[0,:]
x2 = X[1,:]
x3 = X[2,:]
x4 = X[3,:]

U = opti2.variable(3,N)   # control trajectory (throttle)
u1   = U[0,:]
u2 = U[1,:]
u3 = U[2,:]

# ---- objective          ---------
opti2.minimize(x4[N]) # Objective as Mayer term

# ---- dynamic constraints --------
f = lambda x,u: vertcat(c_1*u[0]+c_2*u[1]-(x[0])**0.5-  u[2]* (c_3*x[0])**0.5 , (x[0])**0.5- (x[1])**0.5, (x[1])**0.5- (x[2])**0.5 +u[2]* (c_3*x[0])**0.5 , u[1]*0.0+ k_1*(x[1]-k_2)**2 + k_3*(x[2]-k_4)**2)


dt = T/N # length of a control interval
for k in range(N): # loop over control intervals
   # Runge-Kutta 4 integration
   k1 = f(X[:,k],         U[:,k])
   k2 = f(X[:,k]+dt/2*k1, U[:,k])
   k3 = f(X[:,k]+dt/2*k2, U[:,k])
   k4 = f(X[:,k]+dt*k3,   U[:,k])
   x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4) 
   opti2.subject_to(X[:,k+1]==x_next) # close the gaps

# ---- path constraints -----------
opti2.subject_to(U==b_bin_orig) # control is fixed

# ---- boundary conditions --------
opti2.subject_to(x1[0]==2)   # intitial values
opti2.subject_to(x2[0]==2) # ... 
opti2.subject_to(x3[0]==2) # ... 
opti2.subject_to(x4[0]==0) # ... mayer term auxiliary state

#opti.subject_to(pos[-1]==1)  # finish line at position 1

# ---- misc. constraints  ----------
#opti.subject_to(T>=0) # Time must be positive

# ---- initial values for solver ---
opti2.set_initial(sol.value_variables())
# ---- initial values for solver ---
opti2.set_initial(x1, sol1_x1)
opti2.set_initial(x2, sol1_x2)
opti2.set_initial(x3, sol1_x3)
opti2.set_initial(x4, 0)
#opti.set_initial(u1, 0)
#opti.set_initial(u2, 0)
#opti.set_initial(u3, 1)


# ---- solve NLP              ------
opti2.solver("ipopt") # set numerical backend
sol2 = opti2.solve()   # actual solve


######################
#### Plot the binary feasible trajectory solution
#####################

figure()

plt.plot(pl.linspace(0, 12, num=N+1),sol2.value(x1),label="x1")
plt.plot(pl.linspace(0, 12, num=N+1),sol2.value(x2),label="x2")
plt.plot(pl.linspace(0, 12, num=N+1),sol2.value(x3),label="x3")

#plot(limit(sol.value(pos)),'r--',label="speed limit")
plt.step(pl.linspace(0, 12, num=N),sol2.value(U[0,:]),'k',label="w1")
plt.step(pl.linspace(0, 12, num=N),sol2.value(U[1,:]),label="w2")
plt.step(pl.linspace(0, 12, num=N),sol2.value(U[2,:]),label="w3")

plt.xlabel('t')
plt.ylabel('state and control values')
plt.title('Differential state trajectories for binary controls')

#xlabel=("t")
plt.legend(loc="upper left")
#title="
show()
plt.savefig('three_tank_binary_solution.png')

######################
#### Plot also the (CIA) rounding trajectories
#####################

f, (ax1, ax2, ax3) = pl.subplots(3, sharex = True)

ax1.step(t[:-1], b_bin_orig[0,:], label = "w", color = "C0", where = "post")
# ax1.step(t[:-1], b_bin_red[0,:], label = "b_bin_red", color = "C0", linestyle = "dashed", where = "post")
ax1.scatter(t[:-1], b_rel[0,:], label = "a", color = "C0", marker = "x")
ax1.legend(loc = "upper left")
ax1.set_ylabel("w_1")

ax2.step(t[:-1], b_bin_orig[1,:], label = "w", color = "C1", where = "post")
# ax2.step(t[:-1], b_bin_red[1,:], label = "b_bin_red", color = "C1", linestyle = "dashed", where = "post")
ax2.scatter(t[:-1], b_rel[1,:], label = "a", color = "C1", marker = "x")
ax2.legend(loc = "upper left")
ax2.set_ylabel("w_2")

ax3.step(t[:-1], b_bin_orig[2,:], label = "w", color = "C2", where = "post")
# ax3.step(t[:-1], b_bin_red[2,:], label = "b_bin_red", color = "C2", linestyle = "dashed", where = "post")
ax3.scatter(t[:-1], b_rel[2,:], label = "a", color = "C2", marker = "x")
ax3.legend(loc = "upper left")
ax3.set_ylabel("w_3")
ax3.set_xlabel("t")

show()
plt.savefig('three_tank_rounding_solution.png')
 ################################################