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 *
25from pylab import *
26
27# Control
28u = MX.sym("u")
29
30# State
31x = MX.sym("x",3)
32s = x[0] # position
33v = x[1] # speed
34m = x[2] # mass
35
36# ODE right hand side
37sdot = v
38vdot = (u - 0.05 * v*v)/m
39mdot = -0.1*u*u
40xdot = vertcat(sdot,vdot,mdot)
41
42# ODE right hand side function
43f = Function('f', [x,u],[xdot])
44
45# Integrate with Explicit Euler over 0.2 seconds
46dt = 0.01  # Time step
47xj = x
48for j in range(20):
49  fj = f(xj,u)
50  xj += dt*fj
51
52# Discrete time dynamics function
53F = Function('F', [x,u],[xj])
54
55# Number of control segments
56nu = 50
57
58# Control for all segments
59U = MX.sym("U",nu)
60
61# Initial conditions
62X0 = MX([0,0,1])
63
64# Integrate over all intervals
65X=X0
66for k in range(nu):
67  X = F(X,U[k])
68
69# Objective function and constraints
70J = mtimes(U.T,U) # u'*u in Matlab
71G = X[0:2]     # x(1:2) in Matlab
72
73# NLP
74nlp = {'x':U, 'f':J, 'g':G}
75
76# Allocate an NLP solver
77opts = {"ipopt.tol":1e-10, "expand":True}
78solver = nlpsol("solver", "ipopt", nlp, opts)
79arg = {}
80
81# Bounds on u and initial condition
82arg["lbx"] = -0.5
83arg["ubx"] =  0.5
84arg["x0"] =   0.4
85
86# Bounds on g
87arg["lbg"] = [10,0]
88arg["ubg"] = [10,0]
89
90# Solve the problem
91res = solver(**arg)
92
93# Get the solution
94plot(res["x"])
95plot(res["lam_x"])
96grid()
97show()
98