forked from marcino239/gps
-
Notifications
You must be signed in to change notification settings - Fork 0
/
vdp_single_shooting.py
93 lines (77 loc) · 2.65 KB
/
vdp_single_shooting.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
#
# This file is part of CasADi.
#
# CasADi -- A symbolic framework for dynamic optimization.
# Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
# K.U. Leuven. All rights reserved.
# Copyright (C) 2011-2014 Greg Horn
#
# CasADi is free software; you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation; either
# version 3 of the License, or (at your option) any later version.
#
# CasADi is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public
# License along with CasADi; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
#
#
import casadi as ca
import numpy as NP
import matplotlib.pyplot as plt
from operator import itemgetter
nk = 20 # Control discretization
tf = 10.0 # End time
# Declare variables (use scalar graph)
u = ca.SX.sym("u") # control
x = ca.SX.sym("x",2) # states
# ODE right hand side and quadratures
xdot = ca.vertcat( [(1 - x[1]*x[1])*x[0] - x[1] + u, x[0]] )
qdot = x[0]*x[0] + x[1]*x[1] + u*u
# DAE residual function
dae = ca.SXFunction("dae", ca.daeIn(x=x, p=u), ca.daeOut(ode=xdot, quad=qdot))
# Create an integrator
integrator = ca.Integrator("integrator", "cvodes", dae, {"tf":tf/nk})
# All controls (use matrix graph)
x = ca.MX.sym("x",nk) # nk-by-1 symbolic variable
U = ca.vertsplit(x) # cheaper than x[0], x[1], ...
# The initial state (x_0=0, x_1=1)
X = ca.MX([0,1])
# Objective function
f = 0
# Build a graph of integrator calls
for k in range(nk):
X,QF = itemgetter('xf','qf')(integrator({'x0':X,'p':U[k]}))
f += QF
# Terminal constraints: x_0(T)=x_1(T)=0
g = X
# Allocate an NLP solver
opts = {'linear_solver': 'ma27'}
nlp = ca.MXFunction("nlp", ca.nlpIn(x=x), ca.nlpOut(f=f,g=g))
solver = ca.NlpSolver("solver", "ipopt", nlp, opts)
# Solve the problem
sol = solver({"lbx" : -0.75,
"ubx" : 1,
"x0" : 0,
"lbg" : 0,
"ubg" : 0})
# Retrieve the solution
u_opt = NP.array(sol["x"])
print( sol )
# Time grid
tgrid_x = NP.linspace(0,10,nk+1)
tgrid_u = NP.linspace(0,10,nk)
# Plot the results
plt.figure(1)
plt.clf()
plt.plot(tgrid_u,u_opt,'b-')
plt.title("Van der Pol optimization - single shooting")
plt.xlabel('time')
plt.legend(['u trajectory'])
plt.grid()
plt.show()