Source code for optoy.dynamic

#
#     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
#
#

from static import *

[docs]class OptimizationTime(OptimizationObject): """ time """ shorthand = "t" _mapping = {} def __init__(self): self.create(1, "t")
timebase = OptimizationTime() def time(): return timebase def value_time(e, t): try: return DMatrix(e) except: f = MXFunction("f",symvar(e), [e]) return f([t])[0]
[docs]class OptimizationState(OptimizationContinousVariable): """ Create a state variable Parameters ------------------- shape: integer or (integer,integer) Matrix shape of the symbol name: string A name for the symbol to be used in printing. Not required to be unique lb: number Lower bound on the decision variable May also be set after initialization as 'x.lb = number' ub: number Upper bound on the decision variable May also be set after initialization as 'x.ub = number' init: number Initial guess for the optimization solver May also be set after initialization as 'x.init = number' """ shorthand = "x" _mapping = {} lim_mapping = {} def __init__(self, shape=1, lb=-inf, ub=inf, name="x", init=0): OptimizationContinousVariable.__init__( self, shape=shape, lb=lb, ub=ub, name=name, init=init) @classmethod def getDependent(cl, v): newvars = set() if hash(v) in cl._mapping: newvars.update(set(symvar(cl._mapping[hash(v)].dot))) if hash(v) in cl.lim_mapping: newvars.update(set(symvar(cl.lim_mapping[hash(v)]))) return newvars
[docs]class OptimizationControl(OptimizationContinousVariable): """ Create a control variable Parameters ------------------- shape: integer or (integer,integer) Matrix shape of the symbol name: string A name for the symbol to be used in printing. Not required to be unique lb: number Lower bound on the decision variable May also be set after initialization as 'x.lb = number' ub: number Upper bound on the decision variable May also be set after initialization as 'x.ub = number' init: number Initial guess for the optimization solver May also be set after initialization as 'x.init = number' """ shorthand = "u" _mapping = {} lim_mapping = {} def __init__(self, shape=1, lb=-inf, ub=inf, name="u", init=0): OptimizationContinousVariable.__init__( self, shape=shape, lb=lb, ub=ub, name=name, init=init) @classmethod def getDependent(cl, v): newvars = set() if hash(v) in cl.lim_mapping: newvars.update(set(symvar(cl.lim_mapping[hash(v)]))) return newvars
state = OptimizationState control = OptimizationControl
[docs]def ocp(f, gl=[], regularize=[], verbose=False, N=20, T=1.0, periodic=False, integration_intervals=1, exact_hessian=None): """ Solves an optimal control problem (OCP):: mininimze E(x(T),v) x(t), u(t), v subject to dx/dt = f(x(t),u(t),v,p) h(x(t),u(t),v,p) <= 0 r(x(0),x(T),v,p) <= 0 with x states, u controls, p static parameters (constant, not optimized for), v variables (constant, optimized for), f the system dynamics, h the path constraints, and r boundary conditions. In optoy, the system dynamics is specified with the .dot attribute on a state: >>> x = state() >>> x.dot = 1-x**2 Parameters ------------------- N : int, optional number of control intervals T : float, symbolic expression, optional time horizon periodic : bool indicate whether the problem is periodic regularize: list of symbolic vector expressions f : symbolic expression A major objective function. Make use of the .end attribute of expressions gl : list of constraints, optional Equality and inequality constraints can be mixed. Each entry in the constraint list should be lhs<=rhs , lhs>=rhs or lhs==rhs where lhs and rhs are expressions. Path constraints and boundary constraints can be mixed. Use .start and .end to obtain the value of a state at the boundaries verbose : bool, optional Specify the verbosity of the output Returns ------------------- If numerical solution was succesful, returns cost at the optimal solution. Otherwise raises an exception. """ if not isinstance(gl, list): raise Exception("Constraints must be given as a list") f = f + OptimizationParameter() print f.shape # Determine nature of constraints, either g(x)<=0 or g(x)==0 gl_pure, gl_equality = sort_constraints(gl) # Get all symbolic primitives syms = get_primitives([f, T] + gl_pure) # For states and controls, retrieve the limits (value start and end time) lims = [i.start for i in syms["x"]] + [i.end for i in syms["x"]] + \ [i.start for i in syms["u"]] + [i.end for i in syms["u"]] # Create structures states = struct_symMX( [entry(str(hash(i)), shape=i.sparsity()) for i in syms["x"]]) controls = struct_symMX( [entry(str(hash(i)), shape=i.sparsity()) for i in syms["u"]]) variables = struct_symMX( [entry(str(hash(i)), shape=i.sparsity()) for i in syms["v"]]) # Identify extensions if applicable ext_symnames = sorted(set(syms.keys()) - {"x", "u", "v", "p", "T"}) # Create structures for extension variables ext_structures = dict([(k, struct_symMX( [entry(str(hash(i)), shape=i.sparsity()) for i in syms[k]])) for k in ext_symnames]) # Compose a list of extension symbols ext_syms = [] for k in ext_symnames: ext_syms += syms[k] # Identify extension classes ext_cl = [i.__class__ for i in syms["T"]][:1] # Compose the NLP variables X = struct_symMX([entry("V", struct=variables), entry( "X", struct=states, repeat=N + 1), entry("U", struct=controls, repeat=N)]) P = struct_symMX([entry(str(hash(i)), shape=i.sparsity()) for i in syms["p"]]) # Create the ode function ode_out = MXFunction("ode_out",syms["x"] + syms["u"] + syms["p"] + syms["v"] + ext_syms, [((T + 0.0) / N) * vertcat([i.dot for i in syms["x"]])]) # Group together all symbols that are constant over an integration interval nonstates = struct_symMX([entry("u", struct=controls), entry("p", struct=P), entry("v", struct=variables)] + [entry(k, struct=ext_structures[k]) for k in ext_symnames]) # Compose arguments made of states/nonstates parts ode_out_ins = states[...] + nonstates["u", ...] + \ nonstates["p", ...] + nonstates["v", ...] for k in ext_symnames: ode_out_ins += nonstates[k, ...] # Change the ode function signature to accept states/nonstates ode = MXFunction("ode", [states, nonstates], [ ode_out(ode_out_ins)[0]]) # Create the integrator intg = simpleRK(ode, integration_intervals, 4) intg = try_expand(intg) Pw0 = P[...] + X["V", ...] + \ [None if i.nlp_var else DMatrix.zeros(i.shape) for i in ext_syms] # Some extensions may wish to introduce new constraints ext_constr = [] for cl in ext_cl: ext_constr_e, gl, gl_pure, gl_equality = cl.process( intg, syms, N, T, X, P, Pw0, states, gl, gl_pure, gl_equality) ext_constr += ext_constr_e # Set path constraints bounds h_out = MXFunction("h_out",syms["x"] + syms["u"] + syms["p"] + syms["v"] + ext_syms, [a for a in gl_pure if dependsOn(a, veccat(syms["x"] + syms["u"]))]) g_out = MXFunction("g_out",syms["p"] + syms["v"] + ext_syms + lims, [a for a in gl_pure if not dependsOn(a, veccat(syms["x"] + syms["u"]))]) f_out = MXFunction("f_out",syms["p"] + syms["v"] + ext_syms + lims, [f]) reg_out = MXFunction("reg_out",syms["x"] + syms["u"] + syms["p"] + syms["v"] + ext_syms, [sumRows(vertcat([inner_prod(i, i) for i in regularize])) * T / N]) # Expand if possible h_out = try_expand(h_out) g_out = try_expand(g_out) f_out = try_expand(f_out) reg_out = try_expand(reg_out) # Diagnostics if dependsOn(f, veccat(syms["x"])): raise Exception( "Objective function cannot contain pure state variables. Try adding .start or .end") Lims = X["X", 0, ...] + X["X", -1, ...] + X["U", 0, ...] + X["U", -1, ...] # Construct NLP constraints G = struct_MX( [entry(str(i), expr=g) for i, g in enumerate(g_out(Pw0 + Lims))] + [entry("path", expr=[h_out(X["X", k, ...] + X["U", k, ...] + Pw0) for k in range(N)])] + ext_constr + [entry("shooting", expr=[X["X", k + 1] - intg([X["X", k], veccat([X["U", k]] + Pw0),1])[0] for k in range(N)])] + ([entry("periodic", expr=[X["X", -1] - X["X", 0]])] if periodic else []) ) # Build a regularization expression # We dont use a helper state because we wish to directly influence the # objective reg = sumRows( vertcat([reg_out(X["X", k, ...] + X["U", k, ...] + Pw0)[0] for k in range(N)])) if reg.isempty(): reg = 0 # Construct the nlp nlp = MXFunction("nlp", nlpIn( x=X, p=P), nlpOut( f=f_out( Pw0 + Lims)[0] + reg, g=G)) # Some extensions may wish to set default options for cl in ext_cl: exact_hessian = cl.setOptions(exact_hessian) # If there were no objections, default to True if exact_hessian is None: exact_hessian = True options = {} options["hessian_approximation"] = "exact" if exact_hessian else "limited-memory" if not verbose: options["print_time"] = False options["print_level"] = 0 options["verbose"] = False # Allocate an ipopt solver solver = NlpSolver("solver", "ipopt", nlp, options) # Set bounds on variables, set initial value x0 = X(0) lbx = X(0) ubx = X(0) for i in syms["v"]: hs = str(hash(i)) lbx["V", hs] = i.lb ubx["V", hs] = i.ub x0["V", hs] = i.init for j in "xu": for i in syms[j]: hs = str(hash(i)) lbx[j.capitalize(), :, hs] = i.lb ubx[j.capitalize(), :, hs] = i.ub for k in range(N + 1): if k == N and j == "u": continue x0[j.capitalize(), k, hs] = value_time( i.init, t=(k + 0.0) * T.init / N) # Set parameter values par = P(0) for i in syms["p"]: h = str(hash(i)) par[h] = i.value # Set constraint bounds lbg = G(0) ubg = G(0) # Set normal constraints bounds for i, eq in enumerate( [e for g, e in zip(gl, gl_equality) if not dependsOn(g,veccat(syms["x"] + syms["u"]))]): if eq: lbg[str(i)] = ubg[str(i)] = 0 else: lbg[str(i)] = -Inf ubg[str(i)] = 0 # Set path constraints bounds for i, eq in enumerate( [e for g, e in zip(gl, gl_equality) if dependsOn(g, veccat(syms["x"] + syms["u"]))]): if eq: lbg["path", :, i] = ubg["path", :, i] = 0 else: lbg["path", :, i] = -Inf ubg["path", :, i] = 0 # Some extensions may wish to set bounds for cl in ext_cl: cl.setBounds(lbx, ubx, x0, lbg, ubg) lbg["shooting", :] = ubg["shooting", :] = 0 if periodic: lbg["periodic"] = ubg["periodic"] = 0 # Solve the problem numerically sol = solver(x0=x0,lbg=lbg,ubg=ubg,lbx=lbx,ubx=ubx,p=par) # Raise an exception if not converged if solver.getStat('return_status') != "Solve_Succeeded": raise Exception( "Problem failed to solve. Add verbose=True to see what happened.") # Add the solution to the OptimizationObjects opt = X(sol["x"]) # Extract solutions for i in syms["v"]: i.sol = opt["V", str(hash(i))] for i in syms["x"]: i.sol = opt["X", :, str(hash(i))] for i in syms["u"]: i.sol = opt["U", :, str(hash(i))] # Some extensions may wish to extract more solutions for cl in ext_cl: cl.extractSol(solver) # Return optimal cost return float(solver.getOutput("f"))