1# 2# This file is part of CasADi. 3# 4# CasADi -- A symbolic framework for dynamic optimization. 5# Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl, 6# K.U. Leuven. All rights reserved. 7# Copyright (C) 2011-2014 Greg Horn 8# 9# CasADi is free software; you can redistribute it and/or 10# modify it under the terms of the GNU Lesser General Public 11# License as published by the Free Software Foundation; either 12# version 3 of the License, or (at your option) any later version. 13# 14# CasADi is distributed in the hope that it will be useful, 15# but WITHOUT ANY WARRANTY; without even the implied warranty of 16# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17# Lesser General Public License for more details. 18# 19# You should have received a copy of the GNU Lesser General Public 20# License along with CasADi; if not, write to the Free Software 21# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 22# 23# 24from casadi import * 25 26""" 27Solves the following optimal control problem (OCP) in differential-algebraic 28equations (DAE) 29 30minimize integral_{t=0}^{10} x0^2 + x1^2 + u^2 dt 31x0,x1,z,u 32 33subject to dot(x0) == z*x0-x1+u \ 34 dot(x1) == x0 } for 0 <= t <= 10 35 0 == x1^2 + z - 1 / 36 x0(t=0) == 0 37 x1(t=0) == 1 38 x0(t=10) == 0 39 x1(t=10) == 0 40 -0.75 <= u <= 1 for 0 <= t <= 10 41 42The method used is direct multiple shooting. 43 44Joel Andersson, 2015 45""" 46 47# Declare variables 48x0 = SX.sym('x0') 49x1 = SX.sym('x1') 50x = vertcat(x0, x1) # Differential states 51z = SX.sym('z') # Algebraic variable 52u = SX.sym('u') # Control 53 54# Differential equation 55f_x = vertcat(z*x0-x1+u, x0) 56 57# Algebraic equation 58f_z = x1**2 + z - 1 59 60# Lagrange cost term (quadrature) 61f_q = x0**2 + x1**2 + u**2 62 63# Create an integrator 64dae = {'x':x, 'z':z, 'p':u, 'ode':f_x, 'alg':f_z, 'quad':f_q} 65opts = {'tf':0.5} # interval length 66I = integrator('I', 'idas', dae, opts) 67 68# Number of intervals 69nk = 20 70 71# Start with an empty NLP 72w = [] # List of variables 73lbw = [] # Lower bounds on w 74ubw = [] # Upper bounds on w 75G = [] # Constraints 76J = 0 # Cost function 77 78# Initial conditions 79Xk = MX.sym('X0', 2) 80w.append(Xk) 81lbw += [ 0, 1 ] 82ubw += [ 0, 1 ] 83 84# Loop over all intervals 85for k in range(nk): 86 # Local control 87 Uk = MX.sym('U'+str(k)) 88 w.append(Uk) 89 lbw += [-0.75] 90 ubw += [ 1.00] 91 92 # Call integrator function 93 Ik = I(x0=Xk, p=Uk) 94 Xk = Ik['xf'] 95 J = J + Ik['qf'] # Sum quadratures 96 97 # "Lift" the variable 98 X_prev = Xk 99 Xk = MX.sym('X'+str(k+1), 2) 100 w.append(Xk) 101 lbw += [-inf, -inf] 102 ubw += [ inf, inf] 103 G.append(X_prev - Xk) 104 105# Allocate an NLP solver 106nlp = {'x':vertcat(*w), 'f':J, 'g':vertcat(*G)} 107opts = {'ipopt.linear_solver':'ma27'} 108solver = nlpsol('solver', 'ipopt', nlp, opts) 109 110# Pass bounds, initial guess and solve NLP 111sol = solver(lbx = lbw, # Lower variable bound 112 ubx = ubw, # Upper variable bound 113 lbg = 0.0, # Lower constraint bound 114 ubg = 0.0, # Upper constraint bound 115 x0 = 0.0) # Initial guess 116 117# Plot the results 118import matplotlib.pyplot as plt 119plt.figure(1) 120plt.clf() 121plt.plot(linspace(0., 10., nk+1), sol['x'][0::3],'--') 122plt.plot(linspace(0., 10., nk+1), sol['x'][1::3],'-') 123plt.plot(linspace(0., 10., nk), sol['x'][2::3],'-.') 124plt.title('Van der Pol optimization - multiple shooting') 125plt.xlabel('time') 126plt.legend(['x0 trajectory','x1 trajectory','u trajectory']) 127plt.grid() 128plt.show() 129