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.''')
