Three Tank multimode problem (python/casadi)

From mintOC
Jump to: navigation, search

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')
 ################################################
 
 
 
<source>