Three Tank multimode problem (python/casadi)
From mintOC
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>
