def prob_function(self, conditionlist, evidence_info, prior_info): Pnlp = self._Pnlp # Objective likeli = self.evidence_construct(conditionlist, evidence_info, sensitivity=False) prior = self.prior_construct(prior_info) self.prob_func = cas.MXFunction('prob_func', [Pnlp], [likeli, prior]) return cas.MXFunction('prob_func', [Pnlp], [likeli, prior])
def setSolver(self, solver, solverOptions=[], objFunOptions=[], constraintFunOptions=[]): if hasattr(self, '_solver'): raise ValueError("You've already set a solver and you can't change it") if not hasattr(self, '_objective'): raise ValueError("You need to set an objective") # make objective function f = C.MXFunction([self.getDesignVars()], [self._objective]) setFXOptions(f, objFunOptions) f.init() # make constraint function g = C.MXFunction([self.getDesignVars()], [self._constraints.getG()]) setFXOptions(g, constraintFunOptions) g.init() def mkParallelG(): gs = [C.MXFunction([self.getDesignVars()],[gg]) for gg in self._constraints._g] for gg in gs: gg.init() pg = C.Parallelizer(gs) # pg.setOption("parallelization","openmp") pg.setOption("parallelization","serial") # pg.setOption("parallelization","expand") pg.init() dvsDummy = C.msym('dvs',(self.nStates()+self.nActions())*self.nSteps+self.nParams()) g_ = C.MXFunction([dvsDummy],[C.veccat(pg.call([dvsDummy]*len(gs)))]) g_.init() return g_ # parallelG = mkParallelG() # guess = self._initialGuess.vectorize() # parallelG.setInput([x*1.1 for x in guess]) # g.setInput([x*1.1 for x in guess]) # # g.evaluate() # parallelG.evaluate() # print parallelG.output()-g.output() # exit(0) # make solver function # self._solver = solver(f, mkParallelG()) self._solver = solver(f, g) setFXOptions(self._solver, solverOptions) self._solver.init() # set constraints self._solver.setInput(self._constraints.getLb(), C.NLP_LBG) self._solver.setInput(self._constraints.getUb(), C.NLP_UBG) self.setBounds()
def setupSolver(self, solverOpts=[], constraintFunOpts=[], callback=None): if not self.collocationIsSetup: raise ValueError("you forgot to call setupCollocation") g = self._constraints.getG() lbg = self._constraints.getLb() ubg = self._constraints.getUb() # Objective function/constraints of the NLP if not hasattr(self, '_objective'): raise ValueError('need to set objective function') nlp = CS.MXFunction(CS.nlpIn(x=self._dvMap.vectorize()), CS.nlpOut(f=self._objective, g=g)) setFXOptions(nlp, constraintFunOpts) nlp.init() # solver callback (optional) if callback is not None: nd = self._dvMap.vectorize().size() nc = self._constraints.getG().size() c = CS.PyFunction( callback, CS.nlpSolverOut(x=CS.sp_dense(nd, 1), f=CS.sp_dense(1, 1), lam_x=CS.sp_dense(nd, 1), lam_g=CS.sp_dense(nc, 1), lam_p=CS.sp_dense(0, 1), g=CS.sp_dense(nc, 1)), [CS.sp_dense(1, 1)]) c.init() solverOpts.append(("iteration_callback", c)) # Allocate an NLP solver self.solver = CS.IpoptSolver(nlp) # self.solver = CS.WorhpSolver(nlp) # self.solver = CS.SQPMethod(nlp) # Set options setFXOptions(self.solver, solverOpts) # initialize the solver self.solver.init() # Bounds on g self.solver.setInput(lbg, 'lbg') self.solver.setInput(ubg, 'ubg') ## Nonlinear constraint function, for debugging gfcn = CS.MXFunction([self._dvMap.vectorize()], [g]) gfcn.init() setFXOptions(gfcn, constraintFunOpts) self._gfcn = gfcn
def mkParallelG(): gs = [C.MXFunction([self.getDesignVars()],[gg]) for gg in self._constraints._g] for gg in gs: gg.init() pg = C.Parallelizer(gs) # pg.setOption("parallelization","openmp") pg.setOption("parallelization","serial") # pg.setOption("parallelization","expand") pg.init() dvsDummy = C.msym('dvs',(self.nStates()+self.nActions())*self.nSteps+self.nParams()) g_ = C.MXFunction([dvsDummy],[C.veccat(pg.call([dvsDummy]*len(gs)))]) g_.init() return g_
def prior_construct(self, prior_info): Pnlp = self._Pnlp if prior_info['type'] == 'Ridge': L2 = prior_info['L2'] prior = cas.mul(Pnlp.T, Pnlp) * L2 elif prior_info['type'] == 'Gaussian': mean = prior_info['mean'] cov = prior_info['cov'] dev = Pnlp - mean prior = cas.mul(cas.mul(dev.T, np.linalg.inv(cov)), dev) elif prior_info['type'] == 'GP': BEmean = prior_info['BEmean'] BEcov = prior_info['BEcov'] linear_BE2Ea = prior_info['BE2Ea'] Eacov = prior_info['Eacov'] _BE = Pnlp[self._NEa:] _Ea = Pnlp[:self._NEa] Eamean = cas.mul(linear_BE2Ea, _BE) dev_BE = _BE - BEmean dev_Ea = _Ea - Eamean prior = cas.mul(cas.mul(dev_BE.T, np.linalg.inv(BEcov)), dev_BE) + \ cas.mul(cas.mul(dev_Ea.T, np.linalg.inv(Eacov)), dev_Ea) self._prior_ = prior self.prob_func = cas.MXFunction('prob_func', [Pnlp], [prior]) return prior
def _setupQuadratureFunctions(self, symbolicDvs): quadouts = [] for name in self._quadratures: quadouts.extend( list(self._quadratures[name].flatten()[:-self._deg])) self.quadratureFun = C.MXFunction([symbolicDvs], quadouts) self.quadratureFun.init()
def solve_bvp_casadi(self): """ Uses casadi's interface to sundials to solve the boundary value problem using a single-shooting method with automatic differen- tiation. Related to PCSJ code. """ self.bvpint = cs.Integrator('cvodes', self.modlT) self.bvpint.setOption('abstol', self.intoptions['bvp_abstol']) self.bvpint.setOption('reltol', self.intoptions['bvp_reltol']) self.bvpint.setOption('tf', 1) self.bvpint.setOption('disable_internal_warnings', True) self.bvpint.setOption('fsens_err_con', True) self.bvpint.init() # Vector of unknowns [y0, T] V = cs.MX.sym("V", self.neq + 1) y0 = V[:-1] T = V[-1] param = cs.vertcat([self.param, T]) yf = self.bvpint.call(cs.integratorIn(x0=y0, p=param))[0] fout = self.modlT.call(cs.daeIn(t=T, x=y0, p=param))[0] # objective: continuity obj = (yf - y0)**2 # yf and y0 are the same ..i.e. 2 ends of periodic fcn obj.append( fout[0]) # y0 is a peak for state 0, i.e. fout[0] is slope state 0 #set up the matrix we want to solve F = cs.MXFunction([V], [obj]) F.init() guess = np.append(self.y0, self.T) solver = cs.ImplicitFunction('kinsol', F) solver.setOption('abstol', self.intoptions['bvp_ftol']) solver.setOption('strategy', 'linesearch') solver.setOption('exact_jacobian', False) solver.setOption('pretype', 'both') solver.setOption('use_preconditioner', True) if self.intoptions['constraints'] == 'positive': solver.setOption('constraints', (2, ) * (self.neq + 1)) solver.setOption('linear_solver_type', 'dense') solver.init() solver.setInput(guess) solver.evaluate() sol = solver.output().toArray().squeeze() self.y0 = sol[:-1] self.T = sol[-1]
def solveBVP_casadi(self): """ Uses casadi's interface to sundials to solve the boundary value problem using a single-shooting method with automatic differen- tiation. """ # Here we create and initialize the integrator SXFunction self.bvpint = cs.CVodesIntegrator(self.modlT) self.bvpint.setOption('abstol',self.intoptions['bvp_abstol']) self.bvpint.setOption('reltol',self.intoptions['bvp_reltol']) self.bvpint.setOption('tf',1) self.bvpint.setOption('disable_internal_warnings', True) self.bvpint.setOption('fsens_err_con', True) self.bvpint.init() # Vector of unknowns [y0, T] V = cs.msym("V",self.NEQ+1) y0 = V[:-1] T = V[-1] t = cs.msym('t') param = cs.vertcat([self.paramset, T]) yf = self.bvpint.call(cs.integratorIn(x0=y0,p=param))[0] fout = self.modlT.call(cs.daeIn(t=t, x=y0,p=param))[0] obj = (yf - y0)**2 obj.append(fout[0]) F = cs.MXFunction([V],[obj]) F.init() solver = cs.KinsolSolver(F) solver.setOption('abstol',self.intoptions['bvp_ftol']) solver.setOption('ad_mode', "forward") solver.setOption('strategy','linesearch') solver.setOption('numeric_jacobian', True) solver.setOption('exact_jacobian', False) solver.setOption('pretype', 'both') solver.setOption('use_preconditioner', True) solver.setOption('numeric_hessian', True) solver.setOption('constraints', (2,)*(self.NEQ+1)) solver.setOption('verbose', False) solver.setOption('sparse', False) solver.setOption('linear_solver', 'dense') solver.init() solver.output().set(self.y0) solver.solve() self.y0 = solver.output().toArray().squeeze()
def __init__(self, ocp, xDot): (fAll, (f0, outputNames0)) = ocp.dae.outputsFun() self._outputNames0 = outputNames0 self._outputNames = ocp.dae.outputNames() if f0 is None: assert (len(self._outputNames0) == 0) else: assert (len(self._outputNames0) == f0.getNumOutputs()) if fAll is None: assert (len(self._outputNames) == 0) else: assert (len(self._outputNames) == fAll.getNumOutputs()) self._nk = ocp.nk self._nicp = ocp.nicp self._deg = ocp.deg outs = [] for timestepIdx in range(self._nk): for nicpIdx in range(self._nicp): # outputs defined at tau_i0 if f0 is not None: outs += f0.call([ ocp._dvMap.xVec(timestepIdx, nicpIdx=nicpIdx, degIdx=0), ocp._dvMap.uVec(timestepIdx), ocp._dvMap.pVec() ]) # all outputs for degIdx in range(1, self._deg + 1): if fAll is not None: outs += fAll.call([ xDot[timestepIdx, nicpIdx, degIdx], ocp._dvMap.xVec(timestepIdx, nicpIdx=nicpIdx, degIdx=degIdx), ocp._dvMap.zVec(timestepIdx, nicpIdx=nicpIdx, degIdx=degIdx), ocp._dvMap.uVec(timestepIdx), ocp._dvMap.pVec() ]) # make the function self.fEveryOutput = C.MXFunction([ocp._dvMap.vectorize()], outs) self.fEveryOutput.init()
def recalculate_jacobian_functions(self): """ Recalculates the Jacobian functions after a change of the model parameter values. """ par_kinds = [self.op.BOOLEAN_CONSTANT, self.op.BOOLEAN_PARAMETER_DEPENDENT, self.op.BOOLEAN_PARAMETER_INDEPENDENT, self.op.INTEGER_CONSTANT, self.op.INTEGER_PARAMETER_DEPENDENT, self.op.INTEGER_PARAMETER_INDEPENDENT, self.op.REAL_CONSTANT, self.op.REAL_PARAMETER_INDEPENDENT, self.op.REAL_PARAMETER_DEPENDENT] pars = reduce(list.__add__, [list(self.op.getVariables(par_kind)) for par_kind in par_kinds]) #Get the parameters except for startTime and finalTime since their #value can't be evaluated parameter_vars = [par for par in pars if not self.op.get_attr(par, "free") \ and not (par.getName() == 'startTime' or \ par.getName() == 'finalTime')] # Get parameter values par_vars = [par.getVar() for par in parameter_vars] par_vals = [self.op.get_attr(par, "_value") for par in parameter_vars] # Substitute non-free parameters in expressions for their values DAE = casadi.substitute(self._dae, par_vars, par_vals) # Defines the DAEResidual Function self.Fdae = casadi.MXFunction([self._mvar_struct["time"], self._mvar_struct["dx"], self._mvar_struct["x"], self._mvar_struct["c"], self._mvar_struct["u"]], DAE) self.Fdae.init() # Define derivatives self.dF_dxdot = self.Fdae.jacobian(1,0) self.dF_dxdot.init() self.dF_dx = self.Fdae.jacobian(2,0) self.dF_dx.init() self.dF_dc = self.Fdae.jacobian(3,0) self.dF_dc.init() self.dF_du = self.Fdae.jacobian(4,0) self.dF_du.init()
def CheckThermoConsis(self, dE_start, tol=1e-8): ''' True: satify the thermodynamic consistency False: not satisfy ''' if self._thermo_constraint_expression is None: self.build_thermo_constraint(thermoTem=298.15) #print(self._thermo_constraint_expression) Pnlp = self._Pnlp if py == 2: thermo_consis_fxn = cas.MXFunction('ThermoConsisFxn', [Pnlp], [self._thermo_constraint_expression]) thermo_consis_fxn.setInput(dE_start, 'i0') thermo_consis_fxn.evaluate() viol_ = thermo_consis_fxn.getOutput('o0') elif py == 3: thermo_consis_fxn = cas.Function('ThermoConsisFxn', [Pnlp], [self._thermo_constraint_expression]) viol_ = thermo_consis_fxn(dE_start) return len(np.argwhere(viol_ < -tol)) == 0
def __init__(self,ocp,U): (fAll,(f0,outputNames0)) = ocp.dae.outputsFun() self._outputNames0 = outputNames0 self._outputNames = ocp.dae.outputNames() assert (len(self._outputNames0) == f0.getNumOutputs()) assert (len(self._outputNames) == fAll.getNumOutputs()) self._nk = ocp.nk outs = [] for timestepIdx in range(self._nk): if f0 is not None: outs += f0.call([ocp._dvMap.xVec(timestepIdx), U[timestepIdx,:].T, ocp._dvMap.pVec()]) # make the function self.fEveryOutput = C.MXFunction([ocp._dvMap.vectorize(),U],outs) self.fEveryOutput.init()
def init_f_xu(self, model): #dx_dt = casadi.vertcat(self.dx_dt) #zeros = casadi.MX.zeros(len(self.dx_dt), 1) # fxu = casadi.substitute(-self.implicit.residuals, # dx_dt, zeros) fxu = casadi.substitute(-self.implicit.residuals, self.dx_dt[0], casadi.MX(0)) for dxi_dt in self.dx_dt[1:]: fxu = casadi.substitute(fxu, dxi_dt, casadi.MX(0)) # substitute parameters if self.parameters is not None: for i, data_i in enumerate(model._all_parameters.data): fxu = casadi.substitute( fxu, self.parameters[i], casadi.MX(data_i)) Fxu = casadi.MXFunction(self.x + self.u, [fxu]) Fxu.init() return Fxu
def linearize_dae_with_simresult(optProblem, t0, sim_result): """ Linearize a DAE represented by an OptimizationProblem object. The DAE is represented by F(t,dx,x,u,w,p) = 0 and the linearized model is given by E*(dx-dx0) = A*(x-x0) + B*(u-u0) + C*(w-w0) + D*(t-t0) + G*(p-p0) + h where E, A, B, C, D ,G , and h are constant coefficient matrices. The linearization is done around the reference point z0 specified by the user. The matrices are computed by evaluating Jacobians with CasADi. (That is, no numerical finite differences are used in the linearization.) Parameters:: sim_result -- Variable trajectory data use to determine the reference point around which the linearization is done Type: None or pyjmi.common.io.ResultDymolaTextual or pyjmi.common.algorithm_drivers.JMResultBase t0 -- Time for which the linearization is done. Returns:: E -- n_eq_F x n_dx matrix corresponding to dF/ddx. A -- n_eq_F x n_x matrix corresponding to -dF/dx. B -- n_eq_F x n_u matrix corresponding to -dF/du. C -- n_eq_F x n_w matrix corresponding to -dF/dw. D -- n_eq_F x 1 matrix corresponding to -dF/dt G -- n_eq_F x n_p_opt matrix corresponding to -dF/dp h -- n_eq_F x 1 matrix corresponding to F(dx0,x0,u0,w0,t0) RefPoint -- dictionary with the values for the reference point around which the linearization is done """ import casadi #Import in function since this module can be used without casadi optProblem.calculateValuesForDependentParameters() # Get model variable vectors var_kinds = { 'dx': optProblem.DERIVATIVE, 'x': optProblem.DIFFERENTIATED, 'u': optProblem.REAL_INPUT, 'w': optProblem.REAL_ALGEBRAIC } mvar_vectors = { 'dx': N.array([ var for var in optProblem.getVariables(var_kinds['dx']) if not var.isAlias() ]), 'x': N.array([ var for var in optProblem.getVariables(var_kinds['x']) if not var.isAlias() ]), 'u': N.array([ var for var in optProblem.getVariables(var_kinds['u']) if not var.isAlias() ]), 'w': N.array([ var for var in optProblem.getVariables(var_kinds['w']) if not var.isAlias() ]) } # Count variables (uneliminated inputs and free parameters are counted # later) n_var = { 'dx': len(mvar_vectors["dx"]), 'x': len(mvar_vectors["x"]), 'u': len(mvar_vectors["u"]), 'w': len(mvar_vectors["w"]) } # Sort parameters par_kinds = [ optProblem.BOOLEAN_CONSTANT, optProblem.BOOLEAN_PARAMETER_DEPENDENT, optProblem.BOOLEAN_PARAMETER_INDEPENDENT, optProblem.INTEGER_CONSTANT, optProblem.INTEGER_PARAMETER_DEPENDENT, optProblem.INTEGER_PARAMETER_INDEPENDENT, optProblem.REAL_CONSTANT, optProblem.REAL_PARAMETER_INDEPENDENT, optProblem.REAL_PARAMETER_DEPENDENT ] pars = reduce( list.__add__, [list(optProblem.getVariables(par_kind)) for par_kind in par_kinds]) mvar_vectors['p_fixed'] = [ par for par in pars if not optProblem.get_attr(par, "free") ] mvar_vectors['p_opt'] = [ par for par in pars if optProblem.get_attr(par, "free") ] n_var['p_opt'] = len(mvar_vectors['p_opt']) # Create named symbolic variable structure named_mvar_struct = OrderedDict() named_mvar_struct["time"] = [optProblem.getTimeVariable()] named_mvar_struct["dx"] = \ [mvar.getVar() for mvar in mvar_vectors['dx']] named_mvar_struct["x"] = \ [mvar.getVar() for mvar in mvar_vectors['x']] named_mvar_struct["w"] = \ [mvar.getVar() for mvar in mvar_vectors['w']] named_mvar_struct["u"] = \ [mvar.getVar() for mvar in mvar_vectors['u']] named_mvar_struct["p_opt"] = \ [mvar.getVar() for mvar in mvar_vectors['p_opt']] # Get parameter values par_vars = [par.getVar() for par in mvar_vectors['p_fixed']] par_vals = [ optProblem.get_attr(par, "_value") for par in mvar_vectors['p_fixed'] ] # Substitute non-free parameters in expressions for their values dae = casadi.substitute([optProblem.getDaeResidual()], par_vars, par_vals) # Substitute named variables with vector variables in expressions named_vars = reduce(list.__add__, named_mvar_struct.values()) mvar_struct = OrderedDict() mvar_struct["time"] = casadi.MX.sym("time") mvar_struct["dx"] = casadi.MX.sym("dx", n_var['dx']) mvar_struct["x"] = casadi.MX.sym("x", n_var['x']) mvar_struct["w"] = casadi.MX.sym("w", n_var['w']) mvar_struct["u"] = casadi.MX.sym("u", n_var['u']) mvar_struct["p_opt"] = casadi.MX.sym("p_opt", n_var['p_opt']) svector_vars = [mvar_struct["time"]] # Create map from name to variable index and type name_map = {} for vt in ["dx", "x", "w", "u", "p_opt"]: i = 0 for var in mvar_vectors[vt]: name = var.getName() name_map[name] = (i, vt) svector_vars.append(mvar_struct[vt][i]) i = i + 1 # DAEResidual in terms of the substituted variables DAE = casadi.substitute(dae, named_vars, svector_vars) # Defines the DAEResidual Function Fdae = casadi.MXFunction([ mvar_struct["time"], mvar_struct["dx"], mvar_struct["x"], mvar_struct["w"], mvar_struct["u"], mvar_struct["p_opt"] ], DAE) Fdae.init() # Define derivatives dF_dt = Fdae.jacobian(0, 0) dF_dt.init() dF_dxdot = Fdae.jacobian(1, 0) dF_dxdot.init() dF_dx = Fdae.jacobian(2, 0) dF_dx.init() dF_dw = Fdae.jacobian(3, 0) dF_dw.init() dF_du = Fdae.jacobian(4, 0) dF_du.init() dF_dp = Fdae.jacobian(5, 0) dF_dp.init() # Compute reference point for the linearization [t0, dotx0, x0, w0, u0, p0] RefPoint = dict() var_kinds = ["dx", "x", "w", "u", "p_opt"] traj = {} for vt in ["dx", "x", "w", "u", "p_opt"]: for var in mvar_vectors[vt]: name = var.getName() try: data = sim_result.result_data.get_variable_data(name) except (pyfmi.common.io.VariableNotFoundError, pyjmi.common.io.VariableNotFoundError): print("Warning: Could not find initial " + "trajectory for variable " + name + ". Using initialGuess attribute value " + "instead.") ordinates = N.array([[op.get_attr(var, "initialGuess")]]) abscissae = N.array([0]) else: abscissae = data.t ordinates = data.x.reshape([-1, 1]) traj[var] = TrajectoryLinearInterpolation(abscissae, ordinates) RefPoint["time"] = t0 for vk in var_kinds: RefPoint[vk] = N.zeros(n_var[vk]) for j in range(len(mvar_vectors[vk])): RefPoint[vk][j] = traj[mvar_vectors[vk][j]].eval(t0)[0][0] #print mvar_vectors[vk][j], "---->", RefPoint[vk][j] #for vk in var_kinds: # print "RefPoint[ "+vk+" ]= ", RefPoint[vk] # Set inputs var_kinds = ["time"] + var_kinds for i, varType in enumerate(var_kinds): dF_dt.setInput(RefPoint[varType], i) dF_dxdot.setInput(RefPoint[varType], i) dF_dx.setInput(RefPoint[varType], i) dF_dw.setInput(RefPoint[varType], i) dF_du.setInput(RefPoint[varType], i) dF_dp.setInput(RefPoint[varType], i) Fdae.setInput(RefPoint[varType], i) # Evaluate derivatives dF_dt.evaluate() dF_dxdot.evaluate() dF_dx.evaluate() dF_dw.evaluate() dF_du.evaluate() dF_dp.evaluate() Fdae.evaluate() # Store result in Matrices D = -dF_dt.getOutput() E = dF_dxdot.getOutput() A = -dF_dx.getOutput() B = -dF_du.getOutput() C = -dF_dw.getOutput() h = Fdae.getOutput() G = -dF_dp.getOutput() return E, A, B, C, D, G, h, RefPoint
def linearize_dae_with_point(optProblem, t0, z0): """ Linearize a DAE represented by an OptimizationProblem object. The DAE is represented by F(dx,x,u,w,t) = 0 and the linearized model is given by E*(dx-dx0) = A*(x-x0) + B*(u-u0) + C*(w-w0) + D*(t-t0) + G*(p-p0) + h where E, A, B, C, D ,G , and h are constant coefficient matrices. The linearization is done around the reference point z0 specified by the user. The matrices are computed by evaluating Jacobians with CasADi. (That is, no numerical finite differences are used in the linearization.) Parameters:: z0 -- Dictionary with the reference point around which the linearization is done. z0['variable_type']= [("variable_name",value),("name",z_r)] z0['x']= [("x1",v1),("x2",v2)...] z0['dx']= [("der(x1)",dv1),("der(x2)",dv2)...] z0['u']= [("u1",uv1),("u2",uv2)...] z0['w']= [("w1",wv1),("w2",wv2)...] z0['p_opt']= [("p1",pv1),("p2",pv2)...] t0 -- Time for which the linearization is done. Returns:: E -- n_eq_F x n_dx matrix corresponding to dF/ddx. A -- n_eq_F x n_x matrix corresponding to -dF/dx. B -- n_eq_F x n_u matrix corresponding to -dF/du. C -- n_eq_F x n_w matrix corresponding to -dF/dw. D -- n_eq_F x 1 matrix corresponding to -dF/dt G -- n_eq_F x n_p_opt matrix corresponding to -dF/dp h -- n_eq_F x 1 matrix corresponding to F(dx0,x0,u0,w0,t0) """ import casadi #Import in function since this module can be used without casadi optProblem.calculateValuesForDependentParameters() # Get model variable vectors var_kinds = { 'dx': optProblem.DERIVATIVE, 'x': optProblem.DIFFERENTIATED, 'u': optProblem.REAL_INPUT, 'w': optProblem.REAL_ALGEBRAIC } mvar_vectors = { 'dx': N.array([ var for var in optProblem.getVariables(var_kinds['dx']) if not var.isAlias() ]), 'x': N.array([ var for var in optProblem.getVariables(var_kinds['x']) if not var.isAlias() ]), 'u': N.array([ var for var in optProblem.getVariables(var_kinds['u']) if not var.isAlias() ]), 'w': N.array([ var for var in optProblem.getVariables(var_kinds['w']) if not var.isAlias() ]) } # Count variables (uneliminated inputs and free parameters are counted # later) n_var = { 'dx': len(mvar_vectors["dx"]), 'x': len(mvar_vectors["x"]), 'u': len(mvar_vectors["u"]), 'w': len(mvar_vectors["w"]) } # Sort parameters par_kinds = [ optProblem.BOOLEAN_CONSTANT, optProblem.BOOLEAN_PARAMETER_DEPENDENT, optProblem.BOOLEAN_PARAMETER_INDEPENDENT, optProblem.INTEGER_CONSTANT, optProblem.INTEGER_PARAMETER_DEPENDENT, optProblem.INTEGER_PARAMETER_INDEPENDENT, optProblem.REAL_CONSTANT, optProblem.REAL_PARAMETER_INDEPENDENT, optProblem.REAL_PARAMETER_DEPENDENT ] pars = reduce( list.__add__, [list(optProblem.getVariables(par_kind)) for par_kind in par_kinds]) mvar_vectors['p_fixed'] = [ par for par in pars if not optProblem.get_attr(par, "free") ] mvar_vectors['p_opt'] = [ par for par in pars if optProblem.get_attr(par, "free") ] n_var['p_opt'] = len(mvar_vectors['p_opt']) # Create named symbolic variable structure named_mvar_struct = OrderedDict() named_mvar_struct["time"] = [optProblem.getTimeVariable()] named_mvar_struct["dx"] = \ [mvar.getVar() for mvar in mvar_vectors['dx']] named_mvar_struct["x"] = \ [mvar.getVar() for mvar in mvar_vectors['x']] named_mvar_struct["w"] = \ [mvar.getVar() for mvar in mvar_vectors['w']] named_mvar_struct["u"] = \ [mvar.getVar() for mvar in mvar_vectors['u']] named_mvar_struct["p_opt"] = \ [mvar.getVar() for mvar in mvar_vectors['p_opt']] # Get parameter values par_vars = [par.getVar() for par in mvar_vectors['p_fixed']] par_vals = [ optProblem.get_attr(par, "_value") for par in mvar_vectors['p_fixed'] ] # Substitute non-free parameters in expressions for their values dae = casadi.substitute([optProblem.getDaeResidual()], par_vars, par_vals) # Substitute named variables with vector variables in expressions named_vars = reduce(list.__add__, named_mvar_struct.values()) mvar_struct = OrderedDict() mvar_struct["time"] = casadi.MX.sym("time") mvar_struct["dx"] = casadi.MX.sym("dx", n_var['dx']) mvar_struct["x"] = casadi.MX.sym("x", n_var['x']) mvar_struct["w"] = casadi.MX.sym("w", n_var['w']) mvar_struct["u"] = casadi.MX.sym("u", n_var['u']) mvar_struct["p_opt"] = casadi.MX.sym("p_opt", n_var['p_opt']) svector_vars = [mvar_struct["time"]] # Create map from name to variable index and type name_map = {} for vt in ["dx", "x", "w", "u", "p_opt"]: i = 0 for var in mvar_vectors[vt]: name = var.getName() name_map[name] = (i, vt) svector_vars.append(mvar_struct[vt][i]) i = i + 1 # DAEResidual in terms of the substituted variables DAE = casadi.substitute(dae, named_vars, svector_vars) # Defines the DAEResidual Function Fdae = casadi.MXFunction([ mvar_struct["time"], mvar_struct["dx"], mvar_struct["x"], mvar_struct["w"], mvar_struct["u"], mvar_struct["p_opt"] ], DAE) Fdae.init() # Define derivatives dF_dt = Fdae.jacobian(0, 0) dF_dt.init() dF_dxdot = Fdae.jacobian(1, 0) dF_dxdot.init() dF_dx = Fdae.jacobian(2, 0) dF_dx.init() dF_dw = Fdae.jacobian(3, 0) dF_dw.init() dF_du = Fdae.jacobian(4, 0) dF_du.init() dF_dp = Fdae.jacobian(5, 0) dF_dp.init() # Compute reference point for the linearization [t0, dotx0, x0, w0, u0, p0] RefPoint = dict() var_kinds = ["dx", "x", "w", "u", "p_opt"] RefPoint["time"] = t0 #Sort Values for reference point stop = False for vt in z0.keys(): RefPoint[vt] = N.zeros(n_var[vt]) passed_indices = list() for var_tuple in z0[vt]: index = name_map[var_tuple[0]][0] value = var_tuple[1] RefPoint[vt][index] = value passed_indices.append(index) missing_indices = [i for i in range(n_var[vt]) \ if i not in passed_indices] if len(missing_indices) != 0: if not stop: sys.stderr.write( "Error: Please provide the value for the following variables in z0:\n" ) for j in missing_indices: v = mvar_vectors[vt][j] name = v.getName() sys.stderr.write(name + "\n") stop = True if stop: sys.exit() missing_types = [vt for vt in var_kinds \ if vt not in z0.keys() and n_var[vt]!=0] if len(missing_types) != 0: sys.stderr.write("Error: Please provide the following types in z0:\n") for j in missing_types: sys.stderr.write(j + "\n") sys.exit() for vk in var_kinds: if n_var[vk] == 0: RefPoint[vk] = N.zeros(n_var[vk]) #for vk in var_kinds: # print "RefPoint[ "+vk+" ]= ", RefPoint[vk] # Set inputs var_kinds = ["time"] + var_kinds for i, varType in enumerate(var_kinds): dF_dt.setInput(RefPoint[varType], i) dF_dxdot.setInput(RefPoint[varType], i) dF_dx.setInput(RefPoint[varType], i) dF_dw.setInput(RefPoint[varType], i) dF_du.setInput(RefPoint[varType], i) dF_dp.setInput(RefPoint[varType], i) Fdae.setInput(RefPoint[varType], i) # Evaluate derivatives dF_dt.evaluate() dF_dxdot.evaluate() dF_dx.evaluate() dF_dw.evaluate() dF_du.evaluate() dF_dp.evaluate() Fdae.evaluate() # Store result in Matrices D = -dF_dt.getOutput() E = dF_dxdot.getOutput() A = -dF_dx.getOutput() B = -dF_du.getOutput() C = -dF_dw.getOutput() h = Fdae.getOutput() G = -dF_dp.getOutput() return E, A, B, C, D, G, h
def setupCollocation(self,tf): if self.collocationIsSetup: raise ValueError("you can't setup collocation twice") self.collocationIsSetup = True ## ----------------------------------------------------------------------------- ## Collocation setup ## ----------------------------------------------------------------------------- # Size of the finite elements self.h = tf/float(self.nk*self.nicp) # make coefficients for collocation/continuity equations self.lagrangePoly = LagrangePoly(deg=self.deg,collPoly=self.collPoly) # function to get h out self.hfun = CS.MXFunction([self._dvMap.vectorize()],[self.h]) self.hfun.init() # add collocation constraints ffcn = self._makeResidualFun() ndiff = self.xSize() nalg = self.zSize() self._xDot = np.resize(np.array([None]),(self.nk,self.nicp,self.deg+1)) # For all finite elements for k in range(self.nk): for i in range(self.nicp): # For all collocation points for j in range(1,self.deg+1): # Get an expression for the state derivative at the collocation point xp_jk = 0 for j2 in range (self.deg+1): # get the time derivative of the differential states (eq 10.19b) xp_jk += self.lagrangePoly.lDotAtTauRoot[j,j2]*self.xVec(k,nicpIdx=i,degIdx=j2) self._xDot[k,i,j] = xp_jk/self.h # Add collocation equations to the NLP [fk] = ffcn.call([self._xDot[k,i,j], self.xVec(k,nicpIdx=i,degIdx=j), self.zVec(k,nicpIdx=i,degIdx=j), self.uVec(k), self.pVec()]) # impose system dynamics (for the differential states (eq 10.19b)) self.constrain(fk,'==',0,tag=("implicit dynamic equation",(k,i,j))) # Get an expression for the state at the end of the finite element xf_k = 0 for j in range(self.deg+1): xf_k += self.lagrangePoly.lAtOne[j]*self.xVec(k,nicpIdx=i,degIdx=j) # print "self.lagrangePoly.lAtOne["+str(j)+"]:" +str(self.lagrangePoly.lAtOne[j]) # mxfun = CS.MXFunction([self._V],[xf_k]) # mxfun.init() # sxfun = CS.SXFunction(mxfun) # sxfun.init() # print "" # print sxfun.outputSX() # Add continuity equation to NLP if i==self.nicp-1: self.constrain(self.xVec(k+1,nicpIdx=0,degIdx=0), '==', xf_k, tag=("continuity",(k,i))) else: self.constrain(self.xVec(k,nicpIdx=i+1,degIdx=0), '==', xf_k, tag=("continuity",(k,i))) # add outputs self._outputMapGenerator = collmaps.OutputMapGenerator(self, self._xDot) self._outputMap = collmaps.OutputMap(self._outputMapGenerator, self._dvMap.vectorize())
def __init__(self, system = None, \ tu = None, uN = None, \ ty = None, yN = None, pinit = None, \ xinit = None, \ scheme = "radau", \ order = 3): self.tstart_setup = time.time() SetupsBaseClass.__init__(self) if not type(system) is systems.ExplODE: raise TypeError("Setup-method " + self.__class__.__name__ + \ " not allowed for system of type " + str(type(system)) + ".") self.system = system # Dimensions self.nx = system.x.shape[0] self.nu = system.u.shape[0] self.np = system.p.shape[0] self.neps_e = system.eps_e.shape[0] self.neps_u = system.eps_u.shape[0] self.nphi = system.phi.shape[0] if np.atleast_2d(tu).shape[0] == 1: self.tu = np.asarray(tu) elif np.atleast_2d(tu).shape[1] == 1: self.tu = np.squeeze(np.atleast_2d(tu).T) else: raise ValueError("Invalid dimension for argument tu.") if ty == None: self.ty = self.tu elif np.atleast_2d(ty).shape[0] == 1: self.ty = np.asarray(ty) elif np.atleast_2d(ty).shape[1] == 1: self.ty = np.squeeze(np.atleast_2d(ty).T) else: raise ValueError("Invalid dimension for argument ty.") self.nsteps = self.tu.shape[0] - 1 self.scheme = scheme self.order = order self.tauroot = ca.collocationPoints(order, scheme) # Degree of interpolating polynomial self.ntauroot = len(self.tauroot) - 1 # Define the optimization variables self.P = ca.MX.sym("P", self.np) self.X = ca.MX.sym("X", (self.nx * (self.ntauroot+1)), self.nsteps) self.XF = ca.MX.sym("XF", self.nx) self.V = ca.MX.sym("V", self.nphi, self.nsteps+1) if self.neps_e != 0: self.EPS_E = ca.MX.sym("EPS_E", \ (self.neps_e * self.ntauroot), self.nsteps) else: self.EPS_E = ca.DMatrix(0, self.nsteps) if self.neps_u != 0: self.EPS_U = ca.MX.sym("EPS_U", \ (self.neps_u * self.ntauroot), self.nsteps) else: self.EPS_U = ca.DMatrix(0, self.nsteps) # Define bounds and initial values self.check_and_set_initials( \ uN = uN, \ pinit = pinit, \ xinit = xinit) # Set tp the collocation coefficients # Coefficients of the collocation equation self.C = np.zeros((self.ntauroot + 1, self.ntauroot + 1)) # Coefficients of the continuity equation self.D = np.zeros(self.ntauroot + 1) # Dimensionless time inside one control interval tau = ca.SX.sym("tau") # Construct the matrix T that contains all collocation time points self.T = np.zeros((self.nsteps, self.ntauroot + 1)) for k in range(self.nsteps): for j in range(self.ntauroot + 1): self.T[k,j] = self.tu[k] + \ (self.tu[k+1] - self.tu[k]) * self.tauroot[j] self.T = self.T.T # For all collocation points self.lfcns = [] for j in range(self.ntauroot + 1): # Construct Lagrange polynomials to get the polynomial basis # at the collocation point L = 1 for r in range(self.ntauroot + 1): if r != j: L *= (tau - self.tauroot[r]) / \ (self.tauroot[j] - self.tauroot[r]) lfcn = ca.SXFunction("lfcn", [tau],[L]) # Evaluate the polynomial at the final time to get the # coefficients of the continuity equation [self.D[j]] = lfcn([1]) # Evaluate the time derivative of the polynomial at all # collocation points to get the coefficients of the # collocation equation tfcn = lfcn.tangent() for r in range(self.ntauroot + 1): self.C[j,r] = tfcn([self.tauroot[r]])[0] self.lfcns.append(lfcn) # Initialize phiN self.phiN = [] # Initialize measurement function phifcn = ca.MXFunction("phifcn", \ [system.t, system.u, system.x, system.eps_u, system.p], \ [system.phi]) # Initialzie setup of g self.g = [] # Initialize ODE right-hand-side ffcn = ca.MXFunction("ffcn", \ [system.t, system.u, system.x, system.eps_e, system.eps_u, \ system.p], [system.f]) # Collect information for measurement function # Structs to hold variables for later mapped evaluation Tphi = [] Uphi = [] Xphi = [] EPS_Uphi = [] for k in range(self.nsteps): hk = self.tu[k + 1] - self.tu[k] t_meas = self.ty[np.where(np.logical_and( \ self.ty >= self.tu[k], self.ty < self.tu[k + 1]))] for t_meas_j in t_meas: Uphi.append(self.uN[:, k]) EPS_Uphi.append(self.EPS_U[:self.neps_u, k]) if t_meas_j == self.tu[k]: Tphi.append(self.tu[k]) Xphi.append(self.X[:self.nx, k]) else: tau = (t_meas_j - self.tu[k]) / hk x_temp = 0 for r in range(self.ntauroot + 1): x_temp += self.lfcns[r]([tau])[0] * \ self.X[r*self.nx : (r+1) * self.nx, k] Tphi.append(t_meas_j) Xphi.append(x_temp) if self.tu[-1] in self.ty: Tphi.append(self.tu[-1]) Uphi.append(self.uN[:,-1]) Xphi.append(self.XF) EPS_Uphi.append(self.EPS_U[:self.neps_u,-1]) # Mapped calculation of the collocation equations # Collocation nodes hc = ca.MX.sym("hc", 1) tc = ca.MX.sym("tc", self.ntauroot) xc = ca.MX.sym("xc", self.nx * (self.ntauroot+1)) eps_ec = ca.MX.sym("eps_ec", self.neps_e * self.ntauroot) eps_uc = ca.MX.sym("eps_uc", self.neps_u * self.ntauroot) coleqn = ca.vertcat([ \ hc * ffcn([tc[j-1], \ system.u, \ xc[j*self.nx : (j+1)*self.nx], \ eps_ec[(j-1)*self.neps_e : j*self.neps_e], \ eps_uc[(j-1)*self.neps_u : j*self.neps_u], \ system.p])[0] - \ sum([self.C[r,j] * xc[r*self.nx : (r+1)*self.nx] \ for r in range(self.ntauroot + 1)]) \ for j in range(1, self.ntauroot + 1)]) coleqnfcn = ca.MXFunction("coleqnfcn", \ [hc, tc, system.u, xc, eps_ec, eps_uc, system.p], [coleqn]) coleqnfcn = coleqnfcn.expand() [gcol] = coleqnfcn.map([ \ np.atleast_2d((self.tu[1:] - self.tu[:-1])), self.T[1:,:], \ self.uN, self.X, self.EPS_E, self.EPS_U, self.P]) # Continuity nodes xnext = ca.MX.sym("xnext", self.nx) conteqn = xnext - sum([self.D[r] * xc[r*self.nx : (r+1)*self.nx] \ for r in range(self.ntauroot + 1)]) conteqnfcn = ca.MXFunction("conteqnfcn", [xnext, xc], [conteqn]) conteqnfcn = conteqnfcn.expand() [gcont] = conteqnfcn.map([ \ ca.horzcat([self.X[:self.nx, 1:], self.XF]), self.X]) # Stack equality constraints together self.g = ca.veccat([gcol, gcont]) # Evaluation of the measurement function [self.phiN] = phifcn.map( \ [ca.horzcat(k) for k in Tphi, Uphi, Xphi, EPS_Uphi] + \ [self.P]) # self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN]) self.tend_setup = time.time() self.duration_setup = self.tend_setup - self.tstart_setup print('Initialization of ExplODE system sucessful.')
def __init__(self, system = None, \ tu = None, uN = None, \ pinit = None): SetupsBaseClass.__init__(self) if not type(system) is systems.BasicSystem: raise TypeError("Setup-method " + self.__class__.__name__ + \ " not allowed for system of type " + str(type(system)) + ".") self.system = system # Dimensions self.nu = system.u.shape[0] self.np = system.p.shape[0] self.nphi = system.phi.shape[0] if np.atleast_2d(tu).shape[0] == 1: self.tu = np.asarray(tu) elif np.atleast_2d(tu).shape[1] == 1: self.tu = np.squeeze(np.atleast_2d(tu).T) else: raise ValueError("Invalid dimension for argument tu.") self.nsteps = tu.shape[0] # Define the struct holding the variables self.P = ca.MX.sym("P", self.np) self.X = ca.DMatrix(0, self.nsteps) self.XF = ca.DMatrix(0, self.nsteps) self.V = ca.MX.sym("V", self.nphi, self.nsteps) self.EPS_E = ca.DMatrix(0, self.nsteps) self.EPS_U = ca.DMatrix(0, self.nsteps) # Set bounds and initial values self.check_and_set_initials( \ uN = uN, pinit = pinit) # Set up phiN self.phiN = [] phifcn = ca.MXFunction("phifcn", \ [system.t, system.u, system.p], [system.phi]) for k in range(self.nsteps): self.phiN.append(phifcn([self.tu[k], \ self.uN[:, k], self.P])[0]) self.phiN = ca.vertcat(self.phiN) # self.phiNfcn = ca.MXFunction("phiNfcn", [self.Vars], [self.phiN]) # Set up g # TODO! Can/should/must gfcn depend on uN and/or t? gfcn = ca.MXFunction("gfcn", [system.p], [system.g]) self.g = gfcn.call([self.P])[0] self.tend_setup = time.time() self.duration_setup = self.tend_setup - self.tstart_setup print('Initialization of BasicSystem system sucessful.')
def j_dx_dt(self): df_dx = casadi.vertcat([casadi.transpose(casadi.jacobian(self.dae, dxi_dt)) for dxi_dt in self.dx_dt]) Jdx_dt = casadi.MXFunction(self.x + self.dx_dt + self.u, [df_dx]) Jdx_dt.init() return df_dx
def run_simulation(self, \ x0 = None, tsim = None, usim = None, psim = None, method = "rk"): r''' :param x0: initial value for the states :math:`x_0 \in \mathbb{R}^{n_x}` :type x0: list, numpy,ndarray, casadi.DMatrix :param tsim: optional, switching time points for the controls :math:`t_{sim} \in \mathbb{R}^{L}` to be used for the simulation :type tsim: list, numpy,ndarray, casadi.DMatrix :param usim: optional, control values :math:`u_{sim} \in \mathbb{R}^{n_u \times L}` to be used for the simulation :type usim: list, numpy,ndarray, casadi.DMatrix :param psim: optional, parameter set :math:`p_{sim} \in \mathbb{R}^{n_p}` to be used for the simulation :type psim: list, numpy,ndarray, casadi.DMatrix :param method: optional, CasADi integrator to be used for the simulation :type method: str This function performs a simulation of the system for a given parameter set :math:`p_{sim}`, starting from a user-provided initial value for the states :math:`x_0`. If the argument ``psim`` is not specified, the estimated parameter set :math:`\hat{p}` is used. For this, a parameter estimation using :func:`run_parameter_estimation()` has to be done beforehand, of course. By default, the switching time points for the controls :math:`t_u` and the corresponding controls :math:`u_N` will be used for simulation. If desired, other time points :math:`t_{sim}` and corresponding controls :math:`u_{sim}` can be passed to the function. For the moment, the function can only be used for systems of type :class:`pecas.systems.ExplODE`. ''' intro.pecas_intro() print('\n' + 27 * '-' + \ ' PECas system simulation ' + 26 * '-') print('\nPerforming system simulation, this might take some time ...') if not type(self.pesetup.system) is systems.ExplODE: raise NotImplementedError("Until now, this function can only " + \ "be used for systems of type ExplODE.") if x0 == None: raise ValueError("You have to provide an initial value x0 " + \ "to run the simulation.") x0 = np.squeeze(np.asarray(x0)) if np.atleast_1d(x0).shape[0] != self.pesetup.nx: raise ValueError("Wrong dimension for initial value x0.") if tsim == None: tsim = self.pesetup.tu if usim == None: usim = self.pesetup.uN if psim == None: try: psim = self.phat except AttributeError: errmsg = ''' You have to either perform a parameter estimation beforehand to obtain a parameter set that can be used for simulation, or you have to provide a parameter set in the argument psim. ''' raise AttributeError(errmsg) else: if not np.atleast_1d(np.squeeze(psim)).shape[0] == self.pesetup.np: raise ValueError("Wrong dimension for parameter set psim.") fp = ca.MXFunction("fp", \ [self.pesetup.system.t, self.pesetup.system.u, \ self.pesetup.system.x, self.pesetup.system.eps_e, \ self.pesetup.system.eps_u, self.pesetup.system.p], \ [self.pesetup.system.f]) fpeval = fp([\ self.pesetup.system.t, self.pesetup.system.u, \ self.pesetup.system.x, np.zeros(self.pesetup.neps_e), \ np.zeros(self.pesetup.neps_u), psim])[0] fsim = ca.MXFunction("fsim", \ ca.daeIn(t = self.pesetup.system.t, \ x = self.pesetup.system.x, \ p = self.pesetup.system.u), \ ca.daeOut(ode = fpeval)) Xsim = [] Xsim.append(x0) u0 = ca.DMatrix() for k, e in enumerate(tsim[:-1]): try: integrator = ca.Integrator("integrator", method, \ fsim, {"t0": e, "tf": tsim[k+1]}) except RuntimeError as err: errmsg = ''' It seems like you want to use an integration method that is not currently supported by CasADi. Please refer to the CasADi documentation for a list of supported integrators, or use the default RK4-method by not setting the method-argument of the function. ''' raise RuntimeError(errmsg) if not self.pesetup.nu == 0: u0 = usim[:, k] Xk_end = itemgetter('xf')(integrator({'x0': x0, 'p': u0})) Xsim.append(Xk_end) x0 = Xk_end self.Xsim = ca.horzcat(Xsim) print( \ '''System simulation finished.''')
def _bvp_setup(self): """ Set up the casadi ipopt solver for the bvp solution """ # Define some variables deg = self.deg nk = self.nk NV = nk * (deg + 1) * self.NEQ # NLP variable vector V = cs.msym("V", NV) XD = np.resize(np.array([], dtype=cs.MX), (nk, deg + 1)) P = cs.msym("P", self.NP * self.nk) PK = P.reshape((self.nk, self.NP)) offset = 0 for k in range(nk): for j in range(deg + 1): XD[k][j] = V[offset:offset + self.NEQ] offset += self.NEQ # Constraint function for the NLP g = [] lbg = [] ubg = [] # For all finite elements for k in range(nk): # For all collocation points for j in range(1, deg + 1): # Get an expression for the state derivative at the # collocation point xp_jk = 0 for j2 in range(deg + 1): # get the time derivative of the differential states # (eq 10.19b) xp_jk += self.coll_setup_dict['C'][j2][j] * XD[k][j2] # Generate parameter set, accounting for variable # # Add collocation equations to the NLP [fk] = self.rfmod.call( [0., xp_jk / self.h, XD[k][j], PK[k, :].T]) # impose system dynamics (for the differential states # (eq # 10.19b)) g += [fk[:self.NEQ]] lbg.append(np.zeros(self.NEQ)) # equality constraints ubg.append(np.zeros(self.NEQ)) # equality constraints # Get an expression for the state at the end of the finite # element xf_k = 0 for j in range(deg + 1): xf_k += self.coll_setup_dict['D'][j] * XD[k][j] # Add continuity equation to NLP # End = Beginning of next if k + 1 != nk: g += [XD[k + 1][0] - xf_k] # At the last segment, periodicity constraints else: g += [XD[0][0] - xf_k] lbg.append(np.zeros(self.NEQ)) ubg.append(np.zeros(self.NEQ)) # Nonlinear constraint function gfcn = cs.MXFunction([V, P], [cs.vertcat(g)]) # Objective function (periodicity) ofcn = cs.MXFunction([V, P], [cs.sumAll(g[-1]**2)]) ## ---- ## SOLVE THE NLP ## ---- # Allocate an NLP solver self.solver = cs.IpoptSolver(ofcn, gfcn) # Set options self.solver.setOption("expand_f", True) self.solver.setOption("expand_g", True) self.solver.setOption("generate_hessian", True) self.solver.setOption("max_iter", 1000) self.solver.setOption("tol", self.int_opt['bvp_tol']) self.solver.setOption("constr_viol_tol", self.int_opt['bvp_constr_tol']) self.solver.setOption("linear_solver", self.int_opt['bvp_linear_solver']) self.solver.setOption('parametric', True) self.solver.setOption('print_level', self.int_opt['bvp_print_level']) # initialize the self.solver self.solver.init() self.lbg = lbg self.ubg = ubg self.need_bvp_setup = False
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)
def makeSolver(self): # make sure all bounds are set (xuMissing, pMissing) = self._boundMap.getMissing() msg = [] for name in xuMissing: msg.append("you forgot to set a bound on \"" + name + "\" at timesteps: " + str(xuMissing[name])) for name in pMissing: msg.append("you forgot to set a bound on \"" + name + "\"") if len(msg) > 0: raise ValueError('\n'.join(msg)) # constraints: constraints = self._constraints._g constraintLbgs = self._constraints._glb constraintUbgs = self._constraints._gub g = [self._setupDynamicsConstraints()] g = [] h = [] hlbs = [] hubs = [] for k in range(len(constraints)): lb = constraintLbgs[k] ub = constraintUbgs[k] if all(lb == ub): g.append(constraints[k] - lb) # constrain to be zero else: h.append(constraints[k]) hlbs.append(lb) hubs.append(ub) g = C.veccat(g) h = C.veccat(h) hlbs = C.veccat(hlbs) hubs = C.veccat(hubs) # design vars V = self._dvMap.vectorize() # gradient of arbitraryObj if hasattr(self, '_obj'): arbitraryObj = self._obj else: arbitraryObj = 0 gradF = C.gradient(arbitraryObj, V) # hessian of lagrangian: J = 0 for gnf in self._gaussNewtonObjF: J += C.jacobian(gnf, V) hessL = C.mul(J.T, J) + C.jacobian(gradF, V) # equality constraint jacobian jacobG = C.jacobian(g, V) # inequality constraint jacobian jacobH = C.jacobian(h, V) # function which generates everything needed masterFun = C.MXFunction([V], [hessL, gradF, g, jacobG, h, jacobH]) masterFun.init() class JorisError(Exception): pass raise JorisError('JORIS, please read the following comment')
def fwd_simulation(self, dE_start, condition, detail=True, reltol=1e-8, abstol=1e-10, DRX=False, drc_opt={}): TotalPressure = condition.TotalPressure TotalFlow = condition.TotalFlow Tem = condition.Temperature tf = condition.SimulationTime opts = {} opts['tf'] = tf # Simulation time opts['abstol'] = abstol opts['reltol'] = reltol opts['disable_internal_warnings'] = True opts['max_num_steps'] = 1e8 if py == 2: Fint = cas.Integrator('Fint', 'cvodes', self._dae_, opts) elif py == 3: Fint = cas.integrator('Fint', 'cvodes', self._dae_, opts) if condition.InitCoverage == {}: x0 = [0] * (self.nspe - 1) + [1] else: # Construct Coverage x0 = [0] * (self.nspe - 1) + [1] for spe, cov in condition.InitCoverage.items(): idx = get_index_species(spe, self.specieslist) x0[idx - self.ngas] = cov x0[-1] -= cov # Partial Pressure Pinlet = np.zeros(self.ngas) for idx, spe in enumerate(self.specieslist): if spe.phase == 'gaseous': Pinlet[idx] = condition.PartialPressure[str(spe)] if str( spe) in condition.PartialPressure.keys() else 0 P_dae = np.hstack([dE_start, Pinlet, Tem, TotalFlow]) F_sim = Fint(x0=x0, p=P_dae) tor = {} for idx, spe in enumerate(self.specieslist): if spe.phase == 'gaseous': tor[str(spe)] = float(F_sim['xf'][idx] - Pinlet[idx] / TotalPressure * TotalFlow) # Detailed Reaction network data # Evaluate partial pressure and surface coverage self.pressure_value = list( (F_sim['xf'][:self.ngas] / TotalFlow * TotalPressure).full().T[0]) self.coverage_value = list(F_sim['xf'][self.ngas:].full().T[0]) # Evaluate Reaction Rate automatically save to Rate attribute x = self._x p = self._p if py == 2: rate_fxn = cas.SXFunction('rate_fxn', [x, p], [self._rate, self._rfor, self._rrev]) rate_fxn.setInput(F_sim['xf'], 'i0') rate_fxn.setInput(P_dae, 'i1') rate_fxn.evaluate() self.rate_value = {} self.rate_value['rnet'] = rate_fxn.getOutput( 'o0').full().T[0].tolist() self.rate_value['rfor'] = rate_fxn.getOutput( 'o1').full().T[0].tolist() self.rate_value['rrev'] = rate_fxn.getOutput( 'o2').full().T[0].tolist() # Evaluate Reaction Energy ene_fxn = cas.SXFunction('ene_fxn', [x, p], [ self._reaction_energy_expression['activation'], self._reaction_energy_expression['enthalpy'] ]) ene_fxn.setInput(F_sim['xf'], 'i0') ene_fxn.setInput(P_dae, 'i1') ene_fxn.evaluate() self.energy_value = {} self.energy_value['activation'] = list( ene_fxn.getOutput('o0').full().T[0]) self.energy_value['enthalpy'] = list( ene_fxn.getOutput('o1').full().T[0]) # Evaluate Equilibrium Constant and Rate Constant k_fxn = cas.SXFunction('k_fxn', [x, p], [self._Keq, self._Qeq, self._kf, self._kr]) k_fxn.setInput(F_sim['xf'], 'i0') k_fxn.setInput(P_dae, 'i1') k_fxn.evaluate() self.equil_rate_const_value = {} self.equil_rate_const_value['Keq'] = list( k_fxn.getOutput('o0').full().T[0]) self.equil_rate_const_value['Qeq'] = list( k_fxn.getOutput('o1').full().T[0]) self.equil_rate_const_value['kf'] = list( k_fxn.getOutput('o2').full().T[0]) self.equil_rate_const_value['kr'] = list( k_fxn.getOutput('o3').full().T[0]) elif py == 3: rate_fxn = cas.Function('rate_fxn', [x, p], [self._rate, self._rfor, self._rrev]) outs = rate_fxn(F_sim['xf'], P_dae) self.rate_value = {} self.rate_value['rnet'] = outs[0].full().T[0].tolist() self.rate_value['rfor'] = outs[1].full().T[0].tolist() self.rate_value['rrev'] = outs[2].full().T[0].tolist() # Evaluate Reaction Energy ene_fxn = cas.Function('ene_fxn', [x, p], [ self._reaction_energy_expression['activation'], self._reaction_energy_expression['enthalpy'] ]) outs = ene_fxn(F_sim['xf'], P_dae) self.energy_value = {} self.energy_value['activation'] = list(outs[0].full().T[0]) self.energy_value['enthalpy'] = list(outs[1].full().T[0]) # Evaluate Equilibrium Constant and Rate Constant k_fxn = cas.Function('k_fxn', [x, p], [self._Keq, self._Qeq, self._kf, self._kr]) outs = k_fxn(F_sim['xf'], P_dae) self.equil_rate_const_value = {} self.equil_rate_const_value['Keq'] = list(outs[0].full().T[0]) self.equil_rate_const_value['Qeq'] = list(outs[1].full().T[0]) self.equil_rate_const_value['kf'] = list(outs[2].full().T[0]) self.equil_rate_const_value['kr'] = list(outs[3].full().T[0]) xrc, xtrc = [], [] # TODO: degree of rate control if DRX: delG = drc_opt.get('delG', 1) ref_species = drc_opt.get('ref', 'H2(g)') numer = drc_opt.get('numer', 'fwd') tor0 = tor[ref_species] if numer == 'ad': opts = fwd_sensitivity_option(tf=tf, reltol=1e-8, abstol=1e-16) Fint = cas.Integrator('Fint', 'cvodes', self._dae_, opts) Pnlp = self._Pnlp P_dae = cas.vertcat([Pnlp, Pinlet, Tem, TotalFlow]) F_sim = Fint(x0=x0, p=P_dae) ii = [ ii for ii, spe in enumerate(self.specieslist) if str(spe) == ref_species ][0] tor_ad = F_sim['xf'][ ii] - Pinlet[ii] / TotalPressure * TotalFlow # define the jacobian and MX function jac = cas.jacobian(tor_ad, Pnlp) # evaluate the jacobian fjac = cas.MXFunction('fjac', [Pnlp], [jac]) xrc = fjac([np.copy(dE_start)])[0] xrc = xrc / tor0 * (_const.Rg * Tem) / (-1000) xrc = xrc.full()[0].tolist()[:len(self.dEa_index)] for idx, j in enumerate(self.dEa_index): dP = np.copy(dE_start) dP[idx] += delG # Partial Pressure P_dae = np.hstack([dP, Pinlet, Tem, TotalFlow]) F_sim = Fint(x0=x0, p=P_dae) for ii, spe in enumerate(self.specieslist): if str(spe) == ref_species: tor_p = float(F_sim['xf'][ii] - Pinlet[ii] / TotalPressure * TotalFlow) if numer == 'cent': dP = np.copy(dE_start) dP[idx] -= delG # Partial Pressure P_dae = np.hstack([dP, Pinlet, Tem, TotalFlow]) F_sim = Fint(x0=x0, p=P_dae) for ii, spe in enumerate(self.specieslist): if str(spe) == ref_species: tor_n = float(F_sim['xf'][ii] - Pinlet[ii] / TotalPressure * TotalFlow) if numer == 'fwd': xrc.append((tor_p - tor0) / tor0 / (-delG * 1000 / (_const.Rg * Tem))) # xrc.append((np.log(np.abs(tor_p)) - np.log(np.abs(tor0)))/(-delG * 1000 /(_const.Rg * Tem))) if numer == 'cent': xrc.append((tor_p - tor_n) / tor0 / (-2 * delG * 1000 / (_const.Rg * Tem))) # xrc.append((np.log(np.abs(tor_p)) - np.log(np.abs(tor_n)))/(-2 * delG * 1000 /(_const.Rg * Tem))) # XTRC for idx, j in enumerate(self.dBE_index): spe = self.reactionlist[idx] dP = np.copy(dE_start) deltaE = np.zeros(self.nspe) deltaE[j] += delG # propagate through stoichiomatric deltaEa = self.stoimat.dot(deltaE) # dP[:len(self.dEa_index)] -= deltaEa dP[len(self.dEa_index):] += deltaE[self.dBE_index] # Partial Pressure P_dae = np.hstack([dP, Pinlet, Tem, TotalFlow]) F_sim = Fint(x0=x0, p=P_dae) for ii, spe in enumerate(self.specieslist): if str(spe) == ref_species: tor_p = float(F_sim['xf'][ii] - Pinlet[ii] / TotalPressure * TotalFlow) if numer == 'fwd': xtrc.append((tor_p - tor0) / tor0 / (-delG * 1000 / (_const.Rg * Tem))) # xrc.append((np.log(np.abs(tor_p)) - np.log(np.abs(tor0)))/(-delG * 1000 /(_const.Rg * Tem))) if numer == 'cent': dP = np.copy(dE_start) deltaE = np.zeros(self.nspe) deltaE[j] -= delG # propagate through stoichiomatric deltaEa = self.stoimat.dot(deltaE) # dP[:len(self.dEa_index)] -= deltaEa dP[len(self.dEa_index):] += deltaE[self.dBE_index] # Partial Pressure P_dae = np.hstack([dP, Pinlet, Tem, TotalFlow]) F_sim = Fint(x0=x0, p=P_dae) for ii, spe in enumerate(self.specieslist): if str(spe) == ref_species: tor_n = float(F_sim['xf'][ii] - Pinlet[ii] / TotalPressure * TotalFlow) xtrc.append((tor_p - tor_n) / tor0 / (-2 * delG * 1000 / (_const.Rg * Tem))) # RESULT result = {} result['pressure'] = self.pressure_value result['coverage'] = self.coverage_value result['rate'] = self.rate_value result['energy'] = self.energy_value result['equil_rate'] = self.equil_rate_const_value result['xrc'] = xrc result['xtrc'] = xtrc self.xrc = xrc return tor, result
def CollocationSetup(self, warmstart=False): """ Sets up NLP for collocation solution. Constructs initial guess arrays, constructs constraint and objective functions, and otherwise passes arguments to the correct places. This looks really inefficient and is likely unneccessary to run multiple times for repeated runs with new data. Not sure how much time it takes compared to the NLP solution. Run immediately before CollocationSolve. """ # Dimensions of the problem nx = self.NVAR # total number of states ndiff = nx # number of differential states nalg = 0 # number of algebraic states nu = 0 # number of controls # Collocated variables NXD = self.NICP * self.NK * (self.DEG + 1) * ndiff # differential states NXA = self.NICP * self.NK * self.DEG * nalg # algebraic states NU = self.NK * nu # Parametrized controls NV = NXD + NXA + NU + self.NP + self.NMP # Total variables self.NV = NV # NLP variable vector V = cs.msym("V", NV) # All variables with bounds and initial guess vars_lb = np.zeros(NV) vars_ub = np.zeros(NV) vars_init = np.zeros(NV) offset = 0 # # Split NLP vector into useable slices # # Get the parameters P = V[offset:offset + self.NP] vars_init[offset:offset + self.NP] = self.NLPdata['p_init'] vars_lb[offset:offset + self.NP] = self.NLPdata['p_min'] vars_ub[offset:offset + self.NP] = self.NLPdata['p_max'] offset += self.NP # indexing variable # Get collocated states and parametrized control XD = np.resize(np.array([], dtype=cs.MX), (self.NK, self.NICP, self.DEG + 1)) # NB: same name as above XA = np.resize(np.array([], dtype=cs.MX), (self.NK, self.NICP, self.DEG)) # NB: same name as above U = np.resize(np.array([], dtype=cs.MX), self.NK) # Prepare the starting data matrix vars_init, vars_ub, and # vars_lb, by looping over finite elements, states, etc. Also # groups the variables in the large unknown vector V into XD and # XA(unused) for later indexing for k in range(self.NK): # Collocated states for i in range(self.NICP): # for j in range(self.DEG + 1): # Get the expression for the state vector XD[k][i][j] = V[offset:offset + ndiff] if j != 0: XA[k][i][j - 1] = V[offset + ndiff:offset + ndiff + nalg] # Add the initial condition index = (self.DEG + 1) * (self.NICP * k + i) + j if k == 0 and j == 0 and i == 0: vars_init[offset:offset+ndiff] = \ self.NLPdata['xD_init'][index,:] vars_lb[offset:offset+ndiff] = \ self.NLPdata['xDi_min'] vars_ub[offset:offset+ndiff] = \ self.NLPdata['xDi_max'] offset += ndiff else: if j != 0: vars_init[offset:offset+nx] = \ np.append(self.NLPdata['xD_init'][index,:], self.NLPdata['xA_init'][index,:]) vars_lb[offset:offset+nx] = \ np.append(self.NLPdata['xD_min'], self.NLPdata['xA_min']) vars_ub[offset:offset+nx] = \ np.append(self.NLPdata['xD_max'], self.NLPdata['xA_max']) offset += nx else: vars_init[offset:offset+ndiff] = \ self.NLPdata['xD_init'][index,:] vars_lb[offset:offset+ndiff] = \ self.NLPdata['xD_min'] vars_ub[offset:offset+ndiff] = \ self.NLPdata['xD_max'] offset += ndiff # Parametrized controls (unused here) U[k] = V[offset:offset + nu] # Attach these initial conditions to external dictionary self.NLPdata['v_init'] = vars_init self.NLPdata['v_ub'] = vars_ub self.NLPdata['v_lb'] = vars_lb # Setting up the constraint function for the NLP. Over each # collocated state, ensure continuitity and system dynamics g = [] lbg = [] ubg = [] # For all finite elements for k in range(self.NK): for i in range(self.NICP): # For all collocation points for j in range(1, self.DEG + 1): # Get an expression for the state derivative # at the collocation point xp_jk = 0 for j2 in range(self.DEG + 1): # get the time derivative of the differential # states (eq 10.19b) xp_jk += self.C[j2][j] * XD[k][i][j2] # Add collocation equations to the NLP [fk] = self.rfmod.call([ 0., xp_jk / self.h, XD[k][i][j], XA[k][i][j - 1], U[k], P ]) # impose system dynamics (for the differential # states (eq 10.19b)) g += [fk[:ndiff]] lbg.append(np.zeros(ndiff)) # equality constraints ubg.append(np.zeros(ndiff)) # equality constraints # impose system dynamics (for the algebraic states # (eq 10.19b)) (unused) g += [fk[ndiff:]] lbg.append(np.zeros(nalg)) # equality constraints ubg.append(np.zeros(nalg)) # equality constraints # Get an expression for the state at the end of the finite # element xf_k = 0 for j in range(self.DEG + 1): xf_k += self.D[j] * XD[k][i][j] # if i==self.NICP-1: # Add continuity equation to NLP if k + 1 != self.NK: # End = Beginning of next g += [XD[k + 1][0][0] - xf_k] lbg.append(-self.NLPdata['CONtol'] * np.ones(ndiff)) ubg.append(self.NLPdata['CONtol'] * np.ones(ndiff)) else: # At the last segment # Periodicity constraints (only for NEQ) g += [XD[0][0][0][:self.NEQ] - xf_k[:self.NEQ]] lbg.append(-self.NLPdata['CONtol'] * np.ones(self.NEQ)) ubg.append(self.NLPdata['CONtol'] * np.ones(self.NEQ)) # else: # g += [XD[k][i+1][0] - xf_k] # Flatten contraint arrays for last addition lbg = np.concatenate(lbg).tolist() ubg = np.concatenate(ubg).tolist() # Constraint to protect against fixed point solutions if self.NLPdata['FPgaurd'] is True: fout = self.model.call( cs.daeIn(t=self.tgrid[0], x=XD[0, 0, 0][:self.NEQ], p=V[:self.NP]))[0] g += [cs.MX(cs.sumAll(fout**2))] lbg.append(np.array(self.NLPdata['FPTOL'])) ubg.append(np.array(cs.inf)) elif self.NLPdata['FPgaurd'] is 'all': fout = self.model.call( cs.daeIn(t=self.tgrid[0], x=XD[0, 0, 0][:self.NEQ], p=V[:self.NP]))[0] g += [cs.MX(fout**2)] lbg += [self.NLPdata['FPTOL']] * self.NEQ ubg += [cs.inf] * self.NEQ # Nonlinear constraint function gfcn = cs.MXFunction([V], [cs.vertcat(g)]) # Minimize derivative of first state variable at t=0 xp_0 = 0 for j in range(self.NLPdata['DEG'] + 1): # get the time derivative of the differential # states (eq 10.19b) xp_0 += self.C[j][0] * XD[0][j][0] obj = xp_0 ofcn = cs.MXFunction([V], [obj]) self.CollocationSolver = cs.IpoptSolver(ofcn, gfcn) for opt, val in self.IpoptOpts.iteritems(): self.CollocationSolver.setOption(opt, val) self.CollocationSolver.setOption('obj_scaling_factor', len(vars_init)) if warmstart: self.CollocationSolver.setOption('warm_start_init_point', 'yes') # initialize the self.CollocationSolver self.CollocationSolver.init() # Initial condition self.CollocationSolver.setInput(vars_init, cs.NLP_X_INIT) # Bounds on x self.CollocationSolver.setInput(vars_lb, cs.NLP_LBX) self.CollocationSolver.setInput(vars_ub, cs.NLP_UBX) # Bounds on g self.CollocationSolver.setInput(np.array(lbg), cs.NLP_LBG) self.CollocationSolver.setInput(np.array(ubg), cs.NLP_UBG) if warmstart: self.CollocationSolver.setInput( \ self.WarmStartData['NLP_X_OPT'],cs.NLP_X_INIT) self.CollocationSolver.setInput( \ self.WarmStartData['NLP_LAMBDA_G'],cs.NLP_LAMBDA_INIT) self.CollocationSolver.setOutput( \ self.WarmStartData['NLP_LAMBDA_X'],cs.NLP_LAMBDA_X)
def _create_jacobian_functions(self): """ Calculate the Jacobian functions of a DAE represented by an OptimizationProblem object. The DAE is represented by F(dx,x,u,c,t) = 0 The matrices are computed by evaluating Jacobians with CasADi. (That is, no numerical finite differences are used in the linearization.) The following functions are created: dF_dxdot: The derivative with respect to the derivative of the state variables. dF_dx: The derivative with respect to the state variables. dF_dc: The derivative function with respect to the algebraic variables. dF_du: The derivative with respect to the control signals. """ #Make sure every parameter has a value self.op.calculateValuesForDependentParameters() self._mvar_vectors = {'dx': N.array([self.op.getVariable('der(' + \ name + ')') for name in self._state_names]), 'x': N.array([self.op.getVariable(name) \ for name in self._state_names]), 'u': N.array([self.op.getVariable(name) \ for name in self._all_input_names]), 'c': N.array([self.op.getVariable(name) \ for name in self._alg_var_names])} self._nvar = {'dx': len(self._mvar_vectors["dx"]), 'x': len(self._mvar_vectors["x"]), 'u': len(self._mvar_vectors["u"]), 'c': len(self._mvar_vectors["c"])} # Sort parameters par_kinds = [self.op.BOOLEAN_CONSTANT, self.op.BOOLEAN_PARAMETER_DEPENDENT, self.op.BOOLEAN_PARAMETER_INDEPENDENT, self.op.INTEGER_CONSTANT, self.op.INTEGER_PARAMETER_DEPENDENT, self.op.INTEGER_PARAMETER_INDEPENDENT, self.op.REAL_CONSTANT, self.op.REAL_PARAMETER_INDEPENDENT, self.op.REAL_PARAMETER_DEPENDENT] pars = reduce(list.__add__, [list(self.op.getVariables(par_kind)) for par_kind in par_kinds]) self._mvar_vectors['p_fixed'] = [par for par in pars if not self.op.get_attr(par, "free")] # Create named symbolic variable structure named_mvar_struct = OrderedDict() named_mvar_struct["time"] = [self.op.getTimeVariable()] named_mvar_struct["dx"] = \ [mvar.getVar() for mvar in self._mvar_vectors['dx']] named_mvar_struct["x"] = \ [mvar.getVar() for mvar in self._mvar_vectors['x']] named_mvar_struct["c"] = \ [mvar.getVar() for mvar in self._mvar_vectors['c']] named_mvar_struct["u"] = \ [mvar.getVar() for mvar in self._mvar_vectors['u']] # Substitute named variables with vector variables in expressions named_vars = reduce(list.__add__, named_mvar_struct.values()) self._mvar_struct = OrderedDict() self._mvar_struct["time"] = casadi.MX.sym("time") self._mvar_struct["dx"] = casadi.MX.sym("dx", self._nvar['dx']) self._mvar_struct["x"] = casadi.MX.sym("x", self._nvar['x']) self._mvar_struct["c"] = casadi.MX.sym("c", self._nvar['c']) self._mvar_struct["u"] = casadi.MX.sym("u", self._nvar['u']) svector_vars=[self._mvar_struct["time"]] # Create map from name to variable index and type self._name_map = {} for vt in ["dx","x", "c", "u"]: i = 0 for var in self._mvar_vectors[vt]: name = var.getName() self._name_map[name] = (i, vt) svector_vars.append(self._mvar_struct[vt][i]) i = i + 1 # DAEResidual in terms of the substituted variables self._dae = casadi.substitute([self.op.getDaeResidual()], named_vars, svector_vars) # Get parameter values par_vars = [par.getVar() for par in self._mvar_vectors['p_fixed']] par_vals = [self.op.get_attr(par, "_value") for par in self._mvar_vectors['p_fixed']] # Substitute non-free parameters in expressions for their values DAE = casadi.substitute(self._dae, par_vars, par_vals) # Defines the DAEResidual Function self.Fdae = casadi.MXFunction([self._mvar_struct["time"], self._mvar_struct["dx"], self._mvar_struct["x"], self._mvar_struct["c"], self._mvar_struct["u"]], DAE) self.Fdae.init() # Define derivatives self.dF_dxdot = self.Fdae.jacobian(1,0) self.dF_dxdot.init() self.dF_dx = self.Fdae.jacobian(2,0) self.dF_dx.init() self.dF_dc = self.Fdae.jacobian(3,0) self.dF_dc.init() self.dF_du = self.Fdae.jacobian(4,0) self.dF_du.init()
def compute_covariance_matrix(self): r''' This function computes the covariance matrix of the estimated parameters from the inverse of the KKT matrix for the parameter estimation problem. This allows then for statements on the quality of the values of the estimated parameters. For efficiency, only the inverse of the relevant part of the matrix is computed using the Schur complement. A more detailed description of this function will follow in future versions. ''' intro.pecas_intro() print('\n' + 20 * '-' + \ ' PECas covariance matrix computation ' + 21 * '-') print(''' Computing the covariance matrix for the estimated parameters, this might take some time ... ''') self.tstart_cov_computation = time.time() try: N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \ self.w.shape[0]) N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \ self.Vars.shape[0] - self.w.shape[0]) hess = ca.blockcat([ [N2, N1], [N1.T, ca.diag(self.w)], ]) # hess = hess + 1e-10 * ca.diag(self.Vars) # J2 can be re-used from parameter estimation, right? J2 = ca.jacobian(self.g, self.Vars) kkt = ca.blockcat( \ [[hess, \ J2.T], \ [J2, \ ca.MX(self.g.size1(), self.g.size1())]] \ ) B1 = kkt[:self.pesetup.np, :self.pesetup.np] E = kkt[self.pesetup.np:, :self.pesetup.np] D = kkt[self.pesetup.np:, self.pesetup.np:] Dinv = ca.solve(D, E, "csparse") F11 = B1 - ca.mul([E.T, Dinv]) self.fbeta = ca.MXFunction("fbeta", [self.Vars], [ca.mul([self.R.T, self.R]) / \ (self.yN.size + self.g.size1() - self.Vars.size())]) [self.beta] = self.fbeta([self.Varshat]) self.fcovp = ca.MXFunction("fcovp", [self.Vars], \ [self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))]) [self.Covp] = self.fcovp([self.Varshat]) print( \ '''Covariance matrix computation finished, run show_results() to visualize.''') except AttributeError as err: errmsg = ''' You must execute run_parameter_estimation() first before the covariance matrix for the estimated parameters can be computed. ''' raise AttributeError(errmsg) finally: self.tend_cov_computation = time.time() self.duration_cov_computation = self.tend_cov_computation - \ self.tstart_cov_computation
def makeSolver(self, endTime, traj=None): # make sure all bounds are set (xMissing, pMissing) = self._boundMap.getMissing() msg = [] for name in xMissing: msg.append("you forgot to set a bound on \"" + name + "\" at timesteps: " + str(xMissing[name])) for name in pMissing: msg.append("you forgot to set a bound on \"" + name + "\"") if len(msg) > 0: raise ValueError('\n'.join(msg)) # constraints: g = self._constraints.getG() glb = self._constraints.getLb() gub = self._constraints.getUb() gDyn = self._setupDynamicsConstraints(endTime, traj) gDynLb = gDynUb = [C.DMatrix.zeros(gg.shape) for gg in gDyn] g = C.veccat([g] + gDyn) glb = C.veccat([glb] + gDynLb) gub = C.veccat([gub] + gDynUb) self.glb = glb self.gub = gub # design vars V = self._dvMap.vectorize() # gradient of arbitraryObj if hasattr(self, '_obj'): arbitraryObj = self._obj else: arbitraryObj = 0 gradF = C.gradient(arbitraryObj, V) # hessian of lagrangian: Js = [C.jacobian(gnf, V) for gnf in self._gaussNewtonObjF] gradFgns = [C.mul(J.T, F) for (F, J) in zip(self._gaussNewtonObjF, Js)] gaussNewtonHess = sum([C.mul(J.T, J) for J in Js]) hessL = gaussNewtonHess + C.jacobian(gradF, V) gradF += sum(gradFgns) # equality/inequality constraint jacobian gfcn = C.MXFunction([V, self._U], [g]) gfcn.init() jacobG = gfcn.jacobian(0, 0) jacobG.init() # function which generates everything needed f = sum([f_ * f_ for f_ in self._gaussNewtonObjF]) if hasattr(self, '_obj'): f += self._obj self.masterFun = C.MXFunction( [V, self._U], [hessL, gradF, g, jacobG.call([V, self._U])[0], f]) self.masterFun.init() # self.qp = C.CplexSolver(hessL.sparsity(),jacobG.output(0).sparsity()) self.qp = C.NLPQPSolver(hessL.sparsity(), jacobG.output(0).sparsity()) self.qp.setOption('nlp_solver', C.IpoptSolver) self.qp.setOption('nlp_solver_options', { 'print_level': 0, 'print_time': False }) self.qp.init()
def idSystem(makePlots=False): # parameters k = 5.0 # spring constant b = 0.4 # viscous damping # noise Q = N.matrix(N.diag([5e-3, 1e-2])**2) R = N.matrix(N.diag([0.03, 0.06])**2) # observation function def h(x): yOut = x return yOut # Time length T = 20.0 # Shooting length nu = 150 # Number of control segments DT = N.double(T) / nu # Create integrator #integrator = create_integrator_cvodes() integrator = create_integrator_rk4() # Initialize the integrator integrator.init() ############# simulation # initial state s0 = 0 # initial position v0 = 1 # initial speed xNext = [s0, v0] Utrue = [] Xtrue = [[s0, v0]] Y = [[s0, v0]] time = [0] for j in range(nu): u = N.sin(N.double(j) / 10.0) integrator.setInput(j * DT, C.INTEGRATOR_T0) integrator.setInput((j + 1) * DT, C.INTEGRATOR_TF) integrator.setInput([u, k, b], C.INTEGRATOR_P) integrator.setInput(xNext, C.INTEGRATOR_X0) integrator.evaluate() xNext = list(integrator.getOutput()) x = list(integrator.getOutput()) y = list(integrator.getOutput()) # state record w = N.random.multivariate_normal([0, 0], Q) x[0] += w[0] x[1] += w[1] # measurement v = N.random.multivariate_normal([0, 0], R) y[0] += v[0] y[1] += v[1] Xtrue.append(x) Y.append(y) Utrue.append(u) time.append(time[-1] + DT) ############## parameter estimation ################# # Integrate over all intervals T0 = C.MX( 0 ) # Beginning of time interval (changed from k*DT due to probable Sundials bug) TF = C.MX( DT ) # End of time interval (changed from (k+1)*DT due to probable Sundials bug) design_variables = C.MX('design_variables', 2 + 2 * (nu + 1)) k_hat = design_variables[0] b_hat = design_variables[1] x_hats = [] for j in range(nu + 1): x_hats.append(design_variables[2 + 2 * j:2 + 2 * j + 2]) logLiklihood = C.MX(0) # logLiklihood += sensor error for j in range(nu + 1): yj = C.MX(2, 1) yj[0, 0] = C.MX(Y[j][0]) yj[1, 0] = C.MX(Y[j][1]) xj = x_hats[j] # ll = 1/2 * err^T * R^-1 * err err = yj - h(xj) ll = -C.MX(0.5) * C.prod(err.T, C.prod(C.MX(R.I), err)) logLiklihood += ll # logLiklihood += dynamics constraint xdot = C.MX('xdot', 2, 1) for j in range(nu): xj = x_hats[j] Xnext = integrator( [T0, TF, xj, C.vertcat([Utrue[j], k_hat, b_hat]), xdot, C.MX()]) err = Xnext - x_hats[j + 1] ll = -C.MX(0.5) * C.prod(err.T, C.prod(C.MX(Q.I), err)) logLiklihood += ll # Objective function F = -logLiklihood # Terminal constraints G = k_hat - C.MX(20.0) # Create the NLP ffcn = C.MXFunction([design_variables], [F]) # objective function gfcn = C.MXFunction([design_variables], [G]) # constraint function # Allocate an NLP solver #solver = C.IpoptSolver(ffcn,gfcn) solver = C.IpoptSolver(ffcn) # Set options solver.setOption("tol", 1e-10) solver.setOption("hessian_approximation", "limited-memory") solver.setOption("derivative_test", "first-order") # initialize the solver solver.init() # Bounds on u and initial condition kb_min = [1, 0.1] + [-10 for j in range(2 * (nu + 1))] # lower bound solver.setInput(kb_min, C.NLP_SOLVER_LBX) kb_max = [10, 1] + [10 for j in range(2 * (nu + 1))] # upper bound solver.setInput(kb_max, C.NLP_SOLVER_UBX) guess = [] for y in Y: guess.append(y[0]) guess.append(y[1]) kb_sol = [5, 0.4] + guess # initial guess solver.setInput(kb_sol, C.NLP_SOLVER_X0) # Bounds on g #Gmin = Gmax = [] #[10, 0] #solver.setInput(Gmin,C.NLP_SOLVER_LBG) #solver.setInput(Gmax,C.NLP_SOLVER_UBG) # Solve the problem solver.solve() # Get the solution xopt = solver.getOutput(C.NLP_SOLVER_X) # estimated parameters: print "" print "(estimated, real) k = (" + str(k) + ", " + str( xopt[0]) + "),\t" + str(100.0 * (k - xopt[0]) / k) + " % error" print "(estimated, real) b = (" + str(b) + ", " + str( xopt[1]) + "),\t" + str(100.0 * (b - xopt[1]) / b) + " % error" # estimated state: s_est = [] v_est = [] for j in range(0, nu + 1): s_est.append(xopt[2 + 2 * j]) v_est.append(xopt[2 + 2 * j + 1]) # make plots if makePlots: plt.figure() plt.clf() plt.subplot(2, 1, 1) plt.xlabel('time') plt.ylabel('position') plt.plot(time, [x[0] for x in Xtrue]) plt.plot(time, [y[0] for y in Y], '.') plt.plot(time, s_est) plt.legend(['true', 'meas', 'est']) plt.subplot(2, 1, 2) plt.xlabel('time') plt.ylabel('velocity') plt.plot(time, [x[1] for x in Xtrue]) plt.plot(time, [y[1] for y in Y], '.') plt.plot(time, v_est) plt.legend(['true', 'meas', 'est']) # plt.subplot(3,1,3) # plt.xlabel('time') # plt.ylabel('control input') # plt.plot(time[:-1], Utrue, '.') plt.show() return {'k': xopt[0], 'b': xopt[1]}
def run_parameter_estimation(self, hessian="gauss-newton"): r''' :param hessian: Method of hessian calculation/approximation; possible values are `gauss-newton` and `exact-hessian` :type hessian: str This functions will run a least squares parameter estimation for the given problem and data set. For this, an NLP of the following structure is set up with a direct collocation approach and solved using IPOPT: .. math:: \begin{aligned} & \text{arg}\,\underset{x, p, v, \epsilon_e, \epsilon_u}{\text{min}} & & \frac{1}{2} \| R(w, v, \epsilon_e, \epsilon_u) \|_2^2 \\ & \text{subject to:} & & R(w, v, \epsilon_e, \epsilon_u) = w^{^\mathbb{1}/_\mathbb{2}} \begin{pmatrix} {v} \\ {\epsilon_e} \\ {\epsilon_u} \end{pmatrix} \\ & & & w = \begin{pmatrix} {w_{v}}^T & {w_{\epsilon_{e}}}^T & {w_{\epsilon_{u}}}^T \end{pmatrix} \\ & & & v_{l} + y_{l} - \phi(t_{l}, u_{l}, x_{l}, p) = 0 \\ & & & (t_{k+1} - t_{k}) f(t_{k,j}, u_{k,j}, x_{k,j}, p, \epsilon_{e,k,j}, \epsilon_{u,k,j}) - \sum_{r=0}^{d} \dot{L}_r(\tau_j) x_{k,r} = 0 \\ & & & x_{k+1,0} - \sum_{r=0}^{d} L_r(1) x_{k,r} = 0 \\ & & & t_{k,j} = t_k + (t_{k+1} - t_{k}) \tau_j \\ & & & L_r(\tau) = \prod_{r=0,r\neq j}^{d} \frac{\tau - \tau_r}{\tau_j - \tau_r}\\ & \text{for:} & & k = 1, \dots, N, ~~~ l = 1, \dots, M, ~~~ j = 1, \dots, d, ~~~ r = 1, \dots, d \\ & & & \tau_j = \text{time points w. r. t. scheme and order} \end{aligned} The status of IPOPT provides information whether the computation could be finished sucessfully. The optimal values for all optimization variables :math:`\hat{x}` can be accessed via the class variable ``LSq.Xhat``, while the estimated parameters :math:`\hat{p}` can also be accessed separately via the class attribute ``LSq.phat``. **Please be aware:** IPOPT finishing sucessfully does not necessarly mean that the estimation results for the unknown parameters are useful for your purposes, it just means that IPOPT was able to solve the given optimization problem. You have in any case to verify your results, e. g. by simulation using the class function :func:`run_simulation`. ''' intro.pecas_intro() print('\n' + 18 * '-' + \ ' PECas least squares parameter estimation ' + 18 * '-') print(''' Starting least squares parameter estimation using IPOPT, this might take some time ... ''') self.tstart_estimation = time.time() g = ca.vertcat([ca.vec(self.pesetup.phiN) - self.yN + \ ca.vec(self.pesetup.V)]) self.R = ca.sqrt(self.w) * \ ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]) if self.pesetup.g.size(): g = ca.vertcat([g, self.pesetup.g]) self.g = g self.Vars = ca.veccat([ self.pesetup.P, \ self.pesetup.X, \ self.pesetup.XF, \ self.pesetup.V, \ self.pesetup.EPS_E, \ self.pesetup.EPS_U, \ ]) nlp = ca.MXFunction("nlp", ca.nlpIn(x=self.Vars), \ ca.nlpOut(f=(0.5 * ca.mul([self.R.T, self.R])), g=self.g)) options = {} options["tol"] = 1e-10 options["linear_solver"] = self.linear_solver if hessian == "gauss-newton": # ipdb.set_trace() gradF = nlp.gradient() jacG = nlp.jacobian("x", "g") # Can't the following be implemented more efficiently?! # gradF.derivative(0, 1) J = ca.jacobian(self.R, self.Vars) sigma = ca.MX.sym("sigma") hessLag = ca.MXFunction("H", \ ca.hessLagIn(x = self.Vars, lam_f = sigma), \ ca.hessLagOut(hess = sigma * ca.mul(J.T, J))) options["hess_lag"] = hessLag options["grad_f"] = gradF options["jac_g"] = jacG elif hessian == "exact-hessian": # let NlpSolver-class compute everything pass else: raise NotImplementedError( \ "Requested method is not implemented. Availabe methods " + \ "are 'gauss-newton' (default) and 'exact-hessian'.") # Initialize the solver, solve the optimization problem solver = ca.NlpSolver("solver", "ipopt", nlp, options) # Store the results of the computation Varsinit = ca.veccat([ self.pesetup.Pinit, \ self.pesetup.Xinit, \ self.pesetup.XFinit, \ self.pesetup.Vinit, \ self.pesetup.EPS_Einit, \ self.pesetup.EPS_Uinit, \ ]) sol = solver(x0=Varsinit, lbg=0, ubg=0) self.Varshat = sol["x"] R_squared_fcn = ca.MXFunction("R_squared_fcn", [self.Vars], [ca.mul([ \ ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]).T, ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U])])]) [self.R_squared] = R_squared_fcn([self.Varshat]) self.tend_estimation = time.time() self.duration_estimation = self.tend_estimation - \ self.tstart_estimation print(''' Parameter estimation finished. Check IPOPT output for status information.''')