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