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