def CorrThermoConsis(self, dE_start, fix_Ea=[], fix_BE=[], print_screen=False):
if self._thermo_constraint_expression is None:
self.build_thermo_constraint(thermoTem=298.15)
Pnlp = self._Pnlp
ini_p = np.hstack([dE_start])
dev = Pnlp - ini_p
if py == 2:
object_fxn = cas.mul(dev.T, dev)
elif py == 3:
object_fxn = cas.dot(dev, dev)
nlp = dict(f=object_fxn, x=Pnlp, g=self._thermo_constraint_expression)
nlpopts = dict()
if py == 2:
nlpopts['max_iter'] = 500
nlpopts['tol'] = 1e-8
nlpopts['expect_infeasible_problem'] = 'yes'
nlpopts['hessian_approximation'] = 'exact'
nlpopts['jac_d_constant'] = 'yes'
elif py == 3:
nlpopts['ipopt.max_iter'] = 500
nlpopts['ipopt.tol'] = 1e-8
nlpopts['ipopt.expect_infeasible_problem'] = 'yes'
nlpopts['ipopt.hessian_approximation'] = 'exact'
nlpopts['ipopt.jac_d_constant'] = 'yes'
if py == 2:
solver = cas.NlpSolver('solver', 'ipopt', nlp, nlpopts)
elif py == 3:
solver = cas.nlpsol('solver', 'ipopt', nlp, nlpopts)
# Bounds and initial guess
lbP = -np.inf * np.ones(self._Np)
ubP = np.inf * np.ones(self._Np)
for i in range(len(fix_Ea)):
if check_index(fix_Ea[i], self.dEa_index):
lbP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
ubP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
for i in range(len(fix_BE)):
if check_index(fix_BE[i], self.dBE_index):
lbP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
ubP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
lbG = 0 * np.ones(2 * self.nrxn)
ubG = np.inf * np.ones(2 * self.nrxn)
if not print_screen:
old_stdout = sys.stdout
sys.stdout = tempfile.TemporaryFile()
solution = solver(x0=ini_p, lbg=lbG, ubg=ubG, lbx=lbP, ubx=ubP)
dE_corr = solution['x'].full().T[0].tolist()
if not print_screen:
sys.stdout = old_stdout
return(dE_corr)
def warm_solve(self, x0=None, lam_x=None, lam_g=None):
"""Solve the collocation problem using an initial guess and basis from
a prior solve. Defaults to using the variables from the solve stored in
_results.
"""
warm_solve_opts = dict(self._solver_opts)
warm_solve_opts["warm_start_init_point"] = "yes"
warm_solve_opts["warm_start_bound_push"] = 1e-6
warm_solve_opts["warm_start_slack_bound_push"] = 1e-6
warm_solve_opts["warm_start_mult_bound_push"] = 1e-6
solver = self._solver = cs.NlpSolver("solver", "ipopt", self._nlp,
warm_solve_opts)
if x0 is None: x0 = self._result['x']
if lam_x is None: lam_x = self._result['lam_x']
if lam_g is None: lam_g = self._result['lam_g']
solver.setInput(x0, 'x0')
solver.setInput(self.var.vars_lb, 'lbx')
solver.setInput(self.var.vars_ub, 'ubx')
solver.setInput(self.col_vars['lbg'], 'lbg')
solver.setInput(self.col_vars['ubg'], 'ubg')
solver.setInput(self.pvar.vars_in, 'p')
solver.setInput(lam_x, 'lam_x0')
solver.setInput(lam_g, 'lam_g0')
solver.setOutput(lam_x, "lam_x")
self._solver.evaluate()
if self._solver.getStat('return_status') != 'Solve_Succeeded':
raise RuntimeWarning('Solve status: {}'.format(
self._solver.getStat('return_status')))
self._result = {
'x' : self._solver.getOutput('x'),
'lam_x' : self._solver.getOutput('lam_x'),
'lam_g' : self._solver.getOutput('lam_g'),
'f' : self._solver.getOutput('f'),
}
# Process the optimal vector
self.var.vars_op = self._result['x']
# Store the optimal solution as initial vectors for the next go-around
self.var.vars_in = self.var.vars_op
try: self._plot_setup()
except AttributeError: pass
return float(self._result['f'])
def findZ(self, u, x0=None, z0=None):
if z0 is None:
z0 = self.z0
else:
z0 = z0 / self.algStateScaling
if x0 is None:
x0 = self.x0
else:
x0 = x0 / self.stateScaling
u = u / self.controlScaling
ocp = self.ocp
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
nlp = C.SXFunction(C.nlpIn(x=ocp.z, p=C.vertcat([ocp.x, ocp.u])),
C.nlpOut(f=C.SX(0), g=algS))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('print_user_options', 'no')
solver.setOption('tol', 1e-10)
solver.setOption('print_level', 0)
solver.setOption('file_print_level', 0)
solver.setOption("max_iter", 200) # IPOPT maximum iterations
solver.init()
solver.setInput(NP.zeros(ocp.z.size()), 'lbg') # g_L
solver.setInput(NP.zeros(ocp.z.size()), 'ubg') # g_U
solver.setInput(z0, 'x0')
solver.setInput(C.vertcat([x0, u]), 'p')
solver.evaluate()
self.z0 = solver.output('x')
return solver.output('x') * self.algStateScaling
def create_nlp(self, solve_opts=None):
default_solve_opts = {
'linear_solver': 'ma27',
'print_level': 0,
'print_time': 0,
}
self._solve_opts = {}
self._solve_opts.update(default_solve_opts)
if solve_opts:
for key, val in solve_opts.iteritems():
self._solve_opts[key] = val
# Initialize arguments for solve method
self._nlp_solver = cs.NlpSolver("solver", "ipopt", self._nlp,
self._solve_opts)
def _initialize_solver(self, **kwargs):
# Initialize NLP object
self._nlp = cs.SXFunction(
'nlp',
cs.nlpIn(x = self.var.vars_sx,
p = self.pvar.vars_sx),
cs.nlpOut(f = self.objective_sx,
g = cs.vertcat(self._constraints_sx)))
opts = {
'max_iter' : 10000,
'linear_solver' : 'ma27'
}
if kwargs is not None: opts.update(kwargs)
self._solver_opts = opts
self._solver = cs.NlpSolver("solver", "ipopt", self._nlp,
self._solver_opts)
self.col_vars['lbg'] = np.concatenate(self._constraints_lb)
self.col_vars['ubg'] = np.concatenate(self._constraints_ub)
# 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)
plt.clf()
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.''')
def mle_estimation(self,
dE_start,
conditionlist,
evidence_info,
prior_info,
constraint=True,
nlptol=1e-2,
maxiter=500,
bfgs=True,
print_level=5,
print_screen=False,
report=None):
if report is not None:
import sys
sys_out = sys.stdout
fp = open(report, 'w')
sys.stdout = fp
print('Evidence Info:')
print(str(evidence_info))
print('--' * 20)
print('Prior Info:')
print(str(prior_info))
print('--' * 20)
Pnlp = self._Pnlp
# Objective
obj = self.evidence_construct(conditionlist, evidence_info) + \
self.prior_construct(prior_info)
print(obj)
if self._thermo_constraint_expression is None:
self.build_thermo_constraint(thermoTem=298.15)
if constraint:
nlp = dict(f=obj, x=Pnlp, g=self._thermo_constraint_expression)
else:
nlp = dict(f=obj, x=Pnlp)
nlpopts = {}
nlpopts['max_iter'] = maxiter
nlpopts['tol'] = nlptol
nlpopts['acceptable_tol'] = nlptol
nlpopts['jac_d_constant'] = 'yes'
nlpopts['expect_infeasible_problem'] = 'yes'
nlpopts['hessian_approximation'] = 'limited-memory'
nlpopts['print_level'] = print_level
solver = cas.NlpSolver('solver', 'ipopt', nlp, nlpopts)
# FIXIT
lbP = np.array(prior_info['lbound'])
ubP = np.array(prior_info['ubound'])
# Thermo dynamic consistency check
lbG = 0 * np.ones(2 * self.nrxn)
ubG = np.inf * np.ones(2 * self.nrxn)
x0 = np.hstack([dE_start])
if constraint:
solution = solver(x0=x0, lbx=lbP, ubx=ubP, lbg=lbG, ubg=ubG)
else:
solution = solver(x0=x0, lbx=lbP, ubx=ubP)
opt_sol = solution['x'].full().T[0].tolist()
obj = solution['f'].full()[0][0]
print('=' * 20)
print('Starting Point:')
print(dE_start)
print('Parameter:')
print(opt_sol)
print('Objective:')
print(obj)
if report is not None:
import sys
fp.close()
sys.stdout = sys_out
return opt_sol, obj
def findConstrainedSteadyState(self,
altUTags,
altUVals,
u0,
x0=None,
z0=None,
consList=[]):
if z0 is not None:
z0 = z0 / self.algStateScaling
else:
z0 = self.z0
if x0 is not None:
x0 = x0 / self.stateScaling
else:
x0 = self.x0
u0 = u0 / self.controlScaling
ocp = self.ocp
measurementScaling = self.measurementScaling
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
consScaling = C.vertcat([ocp.variable(k).nominal for k in consList])
altUScaling = C.vertcat(
[ocp.variable(altUTags[k]).nominal for k in range(len(altUTags))])
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
altU = C.vertcat(
[ocp.variable(altUTags[k]).beq for k in range(len(altUTags))])
altU = C.substitute(
altU, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / altUScaling
mSX = C.vertcat(
[ocp.variable(consList[k]).beq for k in range(len(consList))])
mSX = C.substitute(
mSX, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / consScaling
split = C.SXFunction([C.vertcat([ocp.x, ocp.z, ocp.u])],
[ocp.x, ocp.z, ocp.u])
split.init()
nlp = C.SXFunction(
C.nlpIn(x=C.vertcat([ocp.x, ocp.z, ocp.u])),
C.nlpOut(f=C.SX(0), g=C.vertcat([odeS, algS, altU, mSX])))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('tol', 1e-10)
solver.setOption('print_user_options', 'yes')
solver.setOption("max_iter", 200) # IPOPT maximum iterations
solver.init()
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
]) / stateScaling
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
]) / stateScaling
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
uMin = C.vertcat([
ocp.variable(ocp.u[i].getName()).min.getValue()
for i in range(ocp.u.size())
]) / controlScaling
uMax = C.vertcat([
ocp.variable(ocp.u[i].getName()).max.getValue()
for i in range(ocp.u.size())
]) / controlScaling
cMin = C.vertcat([
ocp.variable(consList[i]).min.getValue()
for i in range(len(consList))
]) / consScaling
cMax = C.vertcat([
ocp.variable(consList[i]).max.getValue()
for i in range(len(consList))
]) / consScaling
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), altUVals, cMin]),
'lbg') # g_L
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), altUVals, cMax]),
'ubg') # g_U
solver.setInput(C.vertcat([xMin, zMin, uMin]), 'lbx') # u_L
solver.setInput(C.vertcat([xMax, zMax, uMax]), 'ubx') # u_U
solver.setInput(C.vertcat([x0, z0, u0]), 'x0')
solver.evaluate()
xzu = solver.output('x')
split.setInput(xzu)
split.evaluate()
x0 = split.getOutput(0)
z0 = split.getOutput(1)
u0 = split.getOutput(2)
self.mSXF.setInput(x0, 'x')
self.mSXF.setInput(z0, 'z')
self.mSXF.setInput(u0, 'p')
self.mSXF.evaluate()
y0 = self.mSXF.getOutput()
return x0 * stateScaling, u0 * controlScaling, z0 * algStateScaling, y0 * measurementScaling
def findOptimalPoint(self, u0, x0=None, z0=None, simCount=0, consList=[]):
if z0 is not None:
z0 = z0 / self.algStateScaling
else:
z0 = self.z0
if x0 is not None:
x0 = x0 / self.stateScaling
else:
x0 = self.x0
ocp = self.ocp
u0 = u0 / self.controlScaling
measurementScaling = self.measurementScaling
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
consScaling = C.vertcat([ocp.variable(k).nominal for k in consList])
for k in range(simCount):
x0, z0, y = self.oneStep(x0 * stateScaling, u * controlScaling,
z0 * algStateScaling)
x0 = x0 / stateScaling
z0 = z0 / algStateScaling
y = y / measurementScaling
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
ltermS = C.substitute(
ocp.lterm, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
mSX = C.vertcat(
[ocp.variable(consList[k]).beq for k in range(len(consList))])
mSX = C.substitute(
mSX, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / consScaling
pathVarNames = [sv.getName() for sv in ocp.beq(ocp.path)]
pathScaling = C.vertcat([ocp.nominal(pv) for pv in pathVarNames])
pathS = C.substitute(
ocp.beq(ocp.beq(ocp.path)), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / pathScaling
split = C.SXFunction([C.vertcat([ocp.u, ocp.x, ocp.z])],
[ocp.u, ocp.x, ocp.z])
split.init()
nlp = C.SXFunction(
C.nlpIn(x=C.vertcat([ocp.u, ocp.x, ocp.z])),
C.nlpOut(f=ltermS, g=C.vertcat([odeS, algS, mSX, pathS])))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('print_user_options', 'no')
solver.setOption('print_level', 5)
solver.setOption("tol", 1e-14)
solver.setOption("max_iter", 300) # IPOPT maximum iterations
solver.init()
uMin = C.vertcat([
ocp.variable(ocp.u[i].getName()).min.getValue()
for i in range(ocp.u.size())
]) / controlScaling
uMax = C.vertcat([
ocp.variable(ocp.u[i].getName()).max.getValue()
for i in range(ocp.u.size())
]) / controlScaling
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
]) / stateScaling
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
]) / stateScaling
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
cMin = C.vertcat([
ocp.variable(consList[i]).min.getValue()
for i in range(len(consList))
]) / consScaling
cMax = C.vertcat([
ocp.variable(consList[i]).max.getValue()
for i in range(len(consList))
]) / consScaling
pathMax = C.vertcat([
ocp.variable(pathVarNames[i]).max.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathMin = C.vertcat([
ocp.variable(pathVarNames[i]).min.getValue()
for i in range(ocp.path.size())
]) / pathScaling
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMin, pathMin]),
'lbg') # g_L
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMax, pathMax]),
'ubg') # g_U
solver.setInput(C.vertcat([uMin, xMin, zMin]), 'lbx') # u_L
solver.setInput(C.vertcat([uMax, xMax, zMax]), 'ubx') # u_U
solver.setInput(C.vertcat([u0, x0, z0]), 'x0')
solver.evaluate()
xz = solver.output('x')
split.setInput(xz)
split.evaluate()
u0 = split.getOutput(0)
x0 = split.getOutput(1)
z0 = split.getOutput(2)
self.mSXF.setInput(x0, 'x')
self.mSXF.setInput(z0, 'z')
self.mSXF.setInput(u0, 'p')
self.mSXF.evaluate()
y0 = self.mSXF.getOutput()
return u0 * controlScaling, x0 * stateScaling, z0 * algStateScaling, y0 * measurementScaling
def findSteadyState(self, u, x0, z0=None, simCount=0, consList=[]):
if z0 is not None:
z0 = z0 / self.algStateScaling
else:
z0 = self.z0
if x0 is not None:
x0 = x0 / self.stateScaling
else:
x0 = self.x0
ocp = self.ocp
u = u / self.controlScaling
measurementScaling = self.measurementScaling
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
consScaling = C.vertcat([ocp.variable(k).nominal for k in consList])
for k in range(simCount):
x0, z0, y = self.oneStep(x0 * stateScaling, u * controlScaling,
z0 * algStateScaling)
x0 = x0 / stateScaling
z0 = z0 / algStateScaling
y = y / measurementScaling
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
mSX = C.vertcat(
[ocp.variable(consList[k]).beq for k in range(len(consList))])
mSX = C.substitute(
mSX, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / consScaling
split = C.SXFunction([C.vertcat([ocp.x, ocp.z])], [ocp.x, ocp.z])
split.init()
nlp = C.SXFunction(C.nlpIn(x=C.vertcat([ocp.x, ocp.z]), p=ocp.u),
C.nlpOut(f=C.SX(0), g=C.vertcat([odeS, algS, mSX])))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('print_user_options', 'no')
solver.setOption('tol', 1e-10)
solver.setOption('print_level', 5)
solver.setOption("max_iter", 2000) # IPOPT maximum iterations
solver.init()
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
]) / stateScaling
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
]) / stateScaling
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
cMin = C.vertcat([
ocp.variable(consList[i]).min.getValue()
for i in range(len(consList))
]) / consScaling
cMax = C.vertcat([
ocp.variable(consList[i]).max.getValue()
for i in range(len(consList))
]) / consScaling
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMin]),
'lbg') # g_L
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMax]),
'ubg') # g_U
solver.setInput(C.vertcat([xMin, zMin]), 'lbx') # u_L
solver.setInput(C.vertcat([xMax, zMax]), 'ubx') # u_U
solver.setInput(C.vertcat([x0, z0]), 'x0')
solver.setInput(u, 'p')
solver.evaluate()
xz = solver.output('x')
split.setInput(xz)
split.evaluate()
x0 = split.getOutput(0)
z0 = split.getOutput(1)
self.mSXF.setInput(x0, 'x')
self.mSXF.setInput(z0, 'z')
self.mSXF.setInput(u, 'p')
self.mSXF.evaluate()
y0 = self.mSXF.getOutput()
return x0 * stateScaling, z0 * algStateScaling, y0 * measurementScaling
# Objective
[final_cost] = lf([V['X', n_sim]])
J = final_cost
# Regularize controls
for k in range(n_sim):
[stage_cost] = l([V['X', k], V['U', k], dt])
J += stage_cost
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
# Create solver
opts = {'linear_solver': 'ma57'}
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# x(t=0) = x0
lbx['X', 0] = ubx['X', 0] = x0
# Solve nlp
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
sol = V(sol['x'])
# Plot policy
fig, ax = plt.subplots(1, 2, figsize=(12, 6))
plot_policy(ax, sol['X', :, 'x'], sol['X', :, 'y'], sol['X', :, 'phi'],
def defineOCP(self,
ocp,
DT=20,
controlCost=0,
xOpt=[],
uOpt=[],
finalStateCost=1,
deltaUCons=[]):
self.ocp = ocp
ocp = self.ocp
self.DT = DT
self.n_k = int(self.ocp.tf / self.DT)
self.controlCost = controlCost
stateScaling = C.vertcat([
ocp.variable(ocp.x[k].getName()).nominal
for k in range(ocp.x.size())
])
algStateScaling = C.vertcat([
ocp.variable(ocp.z[k].getName()).nominal
for k in range(ocp.z.size())
])
controlScaling = C.vertcat([
ocp.variable(ocp.u[k].getName()).nominal
for k in range(ocp.u.size())
])
xOpt = xOpt / stateScaling
uOpt = uOpt / controlScaling
self.xOpt = xOpt
self.uOpt = uOpt
self.stateScaling = C.vertcat([
ocp.variable(ocp.x[k].getName()).nominal
for k in range(ocp.x.size())
])
self.algStateScaling = C.vertcat([
ocp.variable(ocp.z[k].getName()).nominal
for k in range(ocp.z.size())
])
self.controlScaling = C.vertcat([
ocp.variable(ocp.u[k].getName()).nominal
for k in range(ocp.u.size())
])
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
ltermS = C.substitute(
ocp.lterm, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
sysIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
sysOut = C.daeOut(ode=odeS, alg=algS, quad=ltermS)
odeF = C.SXFunction(sysIn, sysOut)
odeF.init()
C.Integrator.loadPlugin("idas")
G = C.Integrator("idas", odeF)
G.setOption("reltol", self.INTG_REL_TOL) #for CVODES and IDAS
G.setOption("abstol", self.INTG_ABS_TOL) #for CVODES and IDAS
G.setOption("max_multistep_order", 5) #for CVODES and IDAS
G.setOption("max_step_size", self.IDAS_MAX_STEP_SIZE) #for IDAS only
G.setOption("tf", self.DT)
self.G = G
#==============================================================================
# G.setOption('verbose',True)
# G.addMonitor('res')
# G.addMonitor('inputs')
# G.addMonitor('outputs')
#G.addMonitor('djacB')
# G.addMonitor('bjacB')
# G.addMonitor('jtimesB')
# G.addMonitor('psetup')
# G.addMonitor('psetupB')
# G.addMonitor('psolveB')
# G.addMonitor('resB')
# G.addMonitor('resS')
# G.addMonitor('rhsQB')
#==============================================================================
G.init()
self.n_u = self.ocp.u.size()
self.n_x = self.ocp.x.size()
self.n_v = self.n_u * self.n_k + self.n_x * self.n_k
self.V = C.MX.sym("V", int(self.n_v), 1)
self.U, self.X = self.splitVariables(self.V)
uMin = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).min.getValue()
for i in range(self.n_u)
]) / controlScaling
uMax = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).max.getValue()
for i in range(self.n_u)
]) / controlScaling
UMIN = C.vertcat([uMin for k in range(self.n_k)])
UMAX = C.vertcat([uMax for k in range(self.n_k)])
xMin = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).min.getValue()
for i in range(self.n_x)
]) / stateScaling
xMax = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).max.getValue()
for i in range(self.n_x)
]) / stateScaling
XMIN = C.vertcat([xMin for k in range(self.n_k)])
XMAX = C.vertcat([xMax for k in range(self.n_k)])
if len(deltaUCons) > 0:
addDeltaUCons = True
deltaUCons = deltaUCons / self.controlScaling
else:
addDeltaUCons = False
pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
pathVarNames = [sv.getName() for sv in ocp.beq(ocp.path)]
pathScaling = C.vertcat([ocp.nominal(pv) for pv in pathVarNames])
pathS = C.substitute(
ocp.beq(ocp.beq(ocp.path)), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / pathScaling
pathConstraints = C.SXFunction(pathIn, [pathS])
pathMax = C.vertcat([
ocp.variable(pathVarNames[i]).max.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathMin = C.vertcat([
ocp.variable(pathVarNames[i]).min.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathConstraints.setOption("name", "PATH")
pathConstraints.init()
pathConstraints.setInput(xOpt, 'x')
pathConstraints.setInput([], 'z')
pathConstraints.setInput(uOpt, 'p')
pathConstraints.setInput(0, 't')
pathConstraints.evaluate()
pathOpt = pathConstraints.getOutput()
optimalValues = {}
print 'min <= (name,optimal,nominal) <= max'
for i in range(self.n_x):
print ocp.variable(
ocp.x[i].getName()).min.getValue(), ' <= (', ocp.x[i].getName(
), ',', xOpt[i] * stateScaling[i], ',', stateScaling[
i], ') <= ', ocp.variable(
ocp.x[i].getName()).max.getValue()
optimalValues[ocp.x[i].getName()] = xOpt[i] * stateScaling[i]
for i in range(self.n_u):
print ocp.variable(
ocp.u[i].getName()).min.getValue(), ' <= (', ocp.u[i].getName(
), ',', uOpt[i] * controlScaling[i], ',', controlScaling[
i], ') <= ', ocp.variable(
ocp.u[i].getName()).max.getValue()
if addDeltaUCons:
print -deltaUCons[i] * controlScaling[i], ' <= (Delta(', ocp.u[
i].getName(), ')/DTMPC,', 0, ',', controlScaling[
i], ') <= ', deltaUCons[i] * controlScaling[i]
optimalValues[ocp.u[i].getName()] = uOpt[i] * controlScaling[i]
for i in range(len(pathVarNames)):
print ocp.variable(pathVarNames[i]).min.getValue(
), ' <= (', pathVarNames[i], ',', pathOpt[i] * pathScaling[
i], ',', pathScaling[i], ') <= ', ocp.variable(
pathVarNames[i]).max.getValue()
optimalValues[pathVarNames[i]] = pathOpt[i] * pathScaling[i]
plotTags = [ocp.x[i].getName() for i in range(ocp.x.size())]
plotTags = plotTags + [ocp.u[i].getName() for i in range(ocp.u.size())]
plotTags = plotTags + [sv.getName() for sv in ocp.beq(ocp.path)]
self.plotTags = plotTags
self.optimalValues = optimalValues
# Constraint functions
g = []
g_min = []
g_max = []
self.XU0 = C.MX.sym("XU0", self.n_x + self.n_u, 1)
Z = self.XU0[0:self.n_x]
U0 = self.XU0[self.n_x:self.n_x + self.n_u]
# Build up a graph of integrator calls
obj = 0
zf = C.vertcat([
ocp.variable(ocp.z[k].getName()).start for k in range(ocp.z.size())
]) / algStateScaling
for k in range(self.n_k):
Z, QF, zf = C.integratorOut(
G(C.integratorIn(x0=Z, p=self.U[k], z0=zf)), "xf", "qf", "zf")
errU = self.U[k] - U0
obj = obj + QF + C.mul(C.mul(errU.T, controlCost), errU)
U0 = self.U[k]
# include MS constraints!
g.append(Z - self.X[k])
g_min.append(NP.zeros(self.n_x))
g_max.append(NP.zeros(self.n_x))
Z = self.X[k]
[pathCons] = pathConstraints.call(
C.daeIn(t=[], x=self.X[k], z=zf, p=self.U[k]))
g.append(pathCons) ## be carefull on giving all inputs
g_max.append(pathMax)
g_min.append(pathMin)
if addDeltaUCons:
g.append(errU)
g_max.append(deltaUCons * DT)
g_min.append(-deltaUCons * DT)
#errU = (self.U[-1]-uOpt)
#errX = self.X[-1]-xOpt
#obj = obj + finalStateCost*C.mul((errX).trans(),(errX))+C.mul(C.mul(errU.T,controlCost),errU)
self.obj = obj
### Constrains
g = C.vertcat(g)
nlp = C.MXFunction(C.nlpIn(x=self.V, p=self.XU0), C.nlpOut(f=obj, g=g))
nlp.init()
self.odeF = odeF
self.nlp = nlp
solver = C.NlpSolver('ipopt', nlp)
# remove the comment to implement the hessian
solver.setOption('hessian_approximation',
'limited-memory') # comment for exact hessian
solver.setOption('print_user_options', 'no')
solver.setOption("tol", self.IPOPT_tol) # IPOPT tolerance
solver.setOption("dual_inf_tol",
self.IPOPT_dual_inf_tol) # dual infeasibility
solver.setOption("constr_viol_tol",
self.IPOPT_constr_viol_tol) # primal infeasibility
solver.setOption("compl_inf_tol",
self.IPOPT_compl_inf_tol) # complementarity
# solver.setOption("acceptable_tol",0.01)
# solver.setOption("acceptable_obj_change_tol",1e-6)
# solver.setOption("acceptable_constr_viol_tol",1e-6)
solver.setOption("max_iter",
self.IPOPT_max_iter) # IPOPT maximum iterations
solver.setOption("print_level", self.IPOPT_print_level)
solver.setOption("max_cpu_time",
self.IPOPT_max_cpu_time) # IPOPT maximum iterations
solver.init()
### Variable Bounds and initial guess
solver.setInput(C.vertcat([UMIN, XMIN]), 'lbx') # u_L
solver.setInput(C.vertcat([UMAX, XMAX]), 'ubx') # u_U
solver.setInput(C.vertcat(g_min), 'lbg') # g_L
solver.setInput(C.vertcat(g_max), 'ubg') # g_U
self.solver = solver
u0N = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).initialGuess.getValue()
for i in range(self.n_u)
]) / controlScaling
x0N = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).initialGuess.getValue()
for i in range(self.n_x)
]) / stateScaling
USOL, XSOL = self.forwardSimulation(x0N, u0N)
self.USOL = USOL
self.XSOL = XSOL
def steadyState(self, u, T=0):
ocp = self.ocp
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
])
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
])
x0 = C.vertcat([
ocp.variable(ocp.x[i].getName()).initialGuess.getValue()
for i in range(ocp.x.size())
])
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
])
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
])
z0 = C.vertcat([
ocp.variable(ocp.z[i].getName()).initialGuess.getValue()
for i in range(ocp.z.size())
])
if T > 0:
tout, xh, zh, outs = self.simulatePlot(x0, [u for k in range(T)],
outputVars=None)
x0 = xh[-1]
z0 = zh[-1]
varList = [ocp.x, ocp.z]
varMax = [xMax, zMax]
varMin = [xMin, zMin]
var0 = [x0, z0]
cons = [ocp.ode(ocp.x), ocp.alg]
consMax = [NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size())]
consMin = [NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size())]
C.NlpSolver.loadPlugin('ipopt')
nlp = C.SXFunction(C.nlpIn(x=C.vertcat(varList), p=ocp.u),
C.nlpOut(f=C.SX(0), g=C.vertcat(cons)))
nlp.init()
solver = C.NlpSolver('ipopt', nlp)
solver.setOption("max_iter", 100) # IPOPT maximum iterations
solver.init()
### Variable Bounds and initial guess
solver.setInput(C.vertcat(varMin), 'lbx')
solver.setInput(C.vertcat(varMax), 'ubx')
solver.setInput(C.vertcat(consMin), 'lbg') # g_L
solver.setInput(C.vertcat(consMax), 'ubg') # g_U
solver.setInput(C.vertcat(var0), 'x0')
solver.setInput(u, 'p')
solver.evaluate()
outX = solver.getOutput()
splitF = C.SXFunction([C.vertcat(varList)], varList)
splitF.init()
splitF.setInput(outX)
splitF.evaluate()
xss = splitF.getOutput(0)
zss = splitF.getOutput(1)
return xss, zss
def steadyStateOptimal(self, x0=None, u0=None, z0=None):
ocp = self.ocp
uMin = C.vertcat([
ocp.variable(ocp.u[i].getName()).min.getValue()
for i in range(ocp.u.size())
])
uMax = C.vertcat([
ocp.variable(ocp.u[i].getName()).max.getValue()
for i in range(ocp.u.size())
])
if u0 is None:
u0 = C.vertcat([
ocp.variable(ocp.u[i].getName()).initialGuess.getValue()
for i in range(ocp.u.size())
])
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
])
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
])
if x0 is None:
x0 = C.vertcat([
ocp.variable(ocp.x[i].getName()).initialGuess.getValue()
for i in range(ocp.x.size())
])
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
])
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
])
if z0 is None:
z0 = C.vertcat([
ocp.variable(ocp.z[i].getName()).initialGuess.getValue()
for i in range(ocp.z.size())
])
## PATH CONSTRAINTS
pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
pathConstraints = C.SXFunction(pathIn, [ocp.beq(ocp.path)])
pathMax = [
ocp.variable(ocp.path[i].getName()).max.getValue()
for i in range(ocp.path.size())
]
pathMin = [
ocp.variable(ocp.path[i].getName()).min.getValue()
for i in range(ocp.path.size())
]
pathConstraints.setOption("name", "PATH")
pathConstraints.init()
varList = [ocp.x, ocp.z, ocp.u]
varMax = [xMax, zMax, uMax]
varMin = [xMin, zMin, uMin]
var0 = [x0, z0, u0]
cons = [ocp.ode(ocp.x), ocp.alg, ocp.beq(ocp.path)]
consMax = [
NP.zeros(ocp.x.size()),
NP.zeros(ocp.z.size()),
C.vertcat(pathMax)
]
consMin = [
NP.zeros(ocp.x.size()),
NP.zeros(ocp.z.size()),
C.vertcat(pathMin)
]
C.NlpSolver.loadPlugin('ipopt')
nlp = C.SXFunction(C.nlpIn(x=C.vertcat(varList)),
C.nlpOut(f=ocp.lterm, g=C.vertcat(cons)))
nlp.init()
solver = C.NlpSolver('ipopt', nlp)
solver.setOption("max_iter", 100) # IPOPT maximum iterations
solver.init()
### Variable Bounds and initial guess
solver.setInput(C.vertcat(varMin), 'lbx')
solver.setInput(C.vertcat(varMax), 'ubx')
solver.setInput(C.vertcat(consMin), 'lbg') # g_L
solver.setInput(C.vertcat(consMax), 'ubg') # g_U
solver.setInput(C.vertcat(var0), 'x0') # g_U
solver.evaluate()
outX = solver.getOutput()
splitF = C.SXFunction([C.vertcat(varList)], varList)
splitF.init()
splitF.setInput(outX)
splitF.evaluate()
xOpt = splitF.getOutput(0)
zOpt = splitF.getOutput(1)
uOpt = splitF.getOutput(2)
return uOpt, xOpt, zOpt