def _add_real_parameter(self, name):
"""
Adds a real parameter to the optimization problem object.
Parameters::
name --
The name of the new parameter.
Returns::
par --
The parameter variable
"""
par = mc.RealVariable(self.op, MX.sym(name), mc.Variable.INTERNAL,
mc.Variable.PARAMETER)
self.op.addVariable(par)
return par
def _add_real_variable(self, name):
"""
Adds a real variable to the optimization problem object.
Parameters::
name --
The name of the new variable. Given as a string
Returns::
var --
The added variable
"""
var = mc.RealVariable(self.op, MX.sym(name), mc.Variable.INTERNAL,
mc.Variable.CONTINUOUS)
self.op.addVariable(var)
return var
def _add_real_input(self, name):
"""
Adds a new real input to the optimization problem object.
Parameters::
name --
The name of the new input. Given as a string
Returns::
input --
The added input variable
"""
inp = mc.RealVariable(self.op, MX.sym(name), mc.Variable.INPUT,
mc.Variable.CONTINUOUS)
self.op.addVariable(inp)
return inp
def optimize_for_initial_values(op, x_0_guess, u_0, MHE_opts):
"""
Solves an optimization from time zero to time zero to generate
initial values for the state derivatives and the algebraic
variables.
The input 'op' is mutated. Parameters for startTime and finalTime
are added if they do not already exist. The values for these
parameters are set to zero.
Parameters::
op --
An OptimizationProblem object.
x_0_guess --
A dict containing a guess of the initial state
values. Items are on the form ('name', value).
u_0 --
A dictionary on the form
dict([('input1', u1),...,('inputN', uN)]),
where 'inputX' is the name of the input and uX is the
value of the input at time zero. N is the number of control
signals and X goes from 1 to N.
MHE_opts --
A MHEOptions object. See the documentation of the
options object for more details.
Returns::
dx_0 --
The initial value of the derivative of the state
variables. Given as a list of tuples on the form
(varName, value).
c_0 --
The initial value of the algebraic variables.
Given as a list of tuples on the form
(varName, value)
"""
(x_0_guess, u_0, process_noise_names, undefined_input_names, MHE_opts) = \
_check_inputs(op, x_0_guess, u_0, MHE_opts)
input_names = MHE_opts['input_names']
#Check if there are parameters for start and final time, add if not
st_var = op.getVariable('startTime')
if st_var == None:
par = mc.RealVariable(op, MX.sym('startTime'), mc.Variable.INTERNAL,
mc.Variable.PARAMETER)
op.addVariable(par)
ft_var = op.getVariable('finalTime')
if ft_var == None:
par = mc.RealVariable(op, MX.sym('finalTime'), mc.Variable.INTERNAL,
mc.Variable.PARAMETER)
op.addVariable(par)
#Set the times
op.setStartTime(MX(0.))
op.set('startTime', 0.)
op.setFinalTime(MX(0.))
op.set('finalTime', 0.)
##Set the options for the optimization
opts = op.optimize_options()
#Specifies implicit Euler
opts['n_cp'] = 1
#Set one element between two points
opts['n_e'] = 1
#REMOVE MAYBE
opts["IPOPT_options"]["print_level"] = 0
external_data = _get_eliminated_data_object(u_0, process_noise_names,
undefined_input_names,
input_names)
opts['external_data'] = external_data
#Find the names of the initial state value parameters
initial_value_names = [par.getName() for par in \
op.getVariables(op.REAL_PARAMETER_INDEPENDENT) \
if par.getName().startswith('_start_')]
#Remove the _start_ prefix
state_names = [name[7:] for name in initial_value_names]
#Substitute for non alias variables
state_names = [op.getModelVariable(name).getName() for name in state_names]
for k in range(len(initial_value_names)):
iv_name = initial_value_names[k]
state_name = state_names[k]
op.set(iv_name, x_0_guess[state_name])
res = op.optimize(options=opts)
(dx_0, c_0) = _extract_results(op, res, state_names)
return (dx_0, c_0)
def _soften_constraints(self):
"""
Changes hard variable bounds to soft constraints for all variables for
which the user provided a constraint violation cost when creating the
MPC object.
The softened constraint is accieved by adding a cost to the objective
integrand corresponding to the constraint violation cost * the 1-norm
for each variable.
"""
# Save pathconstraints
path_constr = []
for constr in self.op.getPathConstraints():
path_constr.append(constr)
# Change bounds on variables to soft constraints
for name in list(self.constr_viol_costs.keys()):
var = self.op.getVariable(name)
# Create slack variable
slack_var = casadi.MX.sym("%s_slack" % name)
slack = ci.RealVariable(self.op, slack_var, 0, 3)
slack.setMin(0)
nominal = var.getNominal()
# Check if nominal value is symbolic and find the value
if nominal.isSymbolic():
nominal = self.op.get(nominal.getName())
else:
nominal = nominal.getValue()
if nominal == 0:
print("Warning: Nominal value of base variable is 0. Setting \
nominal for slack variable to 1.")
slack.setNominal(1)
else:
slack.setNominal(0.0001 * N.abs(nominal))
self.op.addVariable(slack)
# Add to Objective Integrand
oi = self.op.getObjectiveIntegrand()
self.op.setObjectiveIntegrand(oi+\
self.constr_viol_costs[name]*slack_var)
# Obtain the bounds
var_min = self.op.get_attr(var, "min")
var_max = self.op.get_attr(var, "max")
# Add constraints and change bounds
if var_min != -N.inf:
var.setMin(-N.inf)
pac_rh = var_min - slack_var
pac_soft = ci.Constraint(var.getVar(), pac_rh, 2)
path_constr.append(pac_soft)
if var_max != N.inf:
var.setMax(N.inf)
pac_rh = var_max + slack_var
pac_soft = ci.Constraint(var.getVar(), pac_rh, 1)
path_constr.append(pac_soft)
self.op.setPathConstraints(path_constr)
def _add_u0(self):
"""
Adds neccessary variables to make blocking factors (du_quad_pen and
du_bounds) valid first sample.
"""
bf = self.options['blocking_factors']
if bf is not None:
for key in list(bf.du_quad_pen.keys()):
var_par = casadi.MX.sym("%s_0" % key)
var = ci.RealVariable(self.op, var_par, 2, 1)
self.op.addVariable(var)
self.op.set("%s_0" % key, 0)
self.extra_param.append("%s_0" % key)
# Find or create new timed variable
var_startTime = self._getTimedVariable(key)
if var_startTime is None:
var_startTime = casadi.MX.sym("%s(startTime)" % key)
st = self.op.getVariable('startTime').getVar()
variable = self.op.getVariable(key)
timedVar_startTime = ci.TimedVariable(
self.op, var_startTime, variable, st)
self.op.addTimedVariable(timedVar_startTime)
# Create new variable
du_quad_pen_par = casadi.MX.sym("%s_du_quad_pen" % key)
du_quad_pen = ci.RealVariable(self.op, du_quad_pen_par, 2, 1)
self.op.addVariable(du_quad_pen)
self.op.set("%s_du_quad_pen" % key, 0)
self.extra_param.append("%s_du_quad_pen" % key)
extra_obj = du_quad_pen_par*(var_startTime-var_par)*\
(var_startTime-var_par)
self.op.setObjective(self.op.getObjective() + extra_obj)
# Save all pointconstraints
pc = []
for constr in self.op.getPointConstraints():
pc.append(constr)
for key in list(bf.du_bounds.keys()):
# Find or create new _0 parameter
var_par = self.op.getVariable("%s_0" % key)
if var_par is None:
var_par = casadi.MX.sym("%s_0" % key)
var = ci.RealVariable(self.op, var_par, 2, 1)
self.op.addVariable(var)
self.op.set("%s_0" % key, 0)
self.extra_param.append("%s_0" % key)
else:
var_par = var_par.getVar()
# Find or create new timed variable
var_startTime = self._getTimedVariable(key)
if var_startTime is None:
var_startTime = casadi.MX.sym("%s(startTime)" % key)
st = self.op.getVariable('startTime').getVar()
variable = self.op.getVariable(key)
timedVar_startTime = ci.TimedVariable(
self.op, var_startTime, variable, st)
self.op.addTimedVariable(timedVar_startTime)
# Create new parameter for pointconstraint bound
du_bounds_par = casadi.MX.sym("%s_du_bounds" % key)
du_bounds = ci.RealVariable(self.op, du_bounds_par, 2, 1)
self.op.addVariable(du_bounds)
self.op.set("%s_du_bounds" % key, 1e10)
self.extra_param.append("%s_du_bounds" % key)
# Create new pointconstraints
bf_constr = var_startTime - var_par
poc1 = ci.Constraint(bf_constr, du_bounds_par, 1)
poc2 = ci.Constraint(bf_constr, -du_bounds_par, 2)
# Append new pointconstraints to list
pc.append(poc1)
pc.append(poc2)
# Set new pointconstraints
self.op.setPointConstraints(pc)
def augment_sensitivities(self, parameters):
"""
Adds forward sensitivity variables and equations for all model variables with respect to given parameters.
Parameters::
parameters --
List of parameter names for which to compute sensitivities.
"""
###################################################################
### This code does not exploit DAE sparsity! ###
### Although this will probably not affect online computations. ###
###################################################################
# Get residuals and variables
dae = self.getDaeResidual()
init = self.getInitialResidual()
var_kinds = {'dx': self.DERIVATIVE,
'x': self.DIFFERENTIATED,
'w': self.REAL_ALGEBRAIC}
mvar_vectors = {'dx': N.array([var for var in self.getVariables(var_kinds['dx'])
if (not var.isAlias() and not var.wasEliminated())]),
'x': N.array([var for var in self.getVariables(var_kinds['x'])
if (not var.isAlias() and not var.wasEliminated())]),
'w': N.array([var for var in self.getVariables(var_kinds['w'])
if (not var.isAlias() and not var.wasEliminated())])}
mvar_par = N.array([self.getVariable(par) for par in parameters])
# Add sensitivity variables
for par in mvar_par:
par_var = par.getVar()
for mvar in mvar_vectors['x']:
# States
mvar_var = mvar.getVar()
name = "d%s/d%s" % (mvar.getName(), par.getName())
sens_var = casadi.MX.sym(name)
sens = ci.RealVariable(self, sens_var, ci.RealVariable.INTERNAL, ci.RealVariable.CONTINUOUS)
self.addVariable(sens)
# State derivatives
dx_mvar = mvar.getMyDerivativeVariable()
dx_mvar_var = dx_mvar.getVar()
dx_name = "der(d%s/d%s)" % (mvar.getName(), par.getName())
dx_sens_var = casadi.MX.sym(dx_name)
dx_sens = ci.DerivativeVariable(self, dx_sens_var, sens)
self.addVariable(dx_sens)
for mvar in mvar_vectors['w']:
# Algebraics
mvar_var = mvar.getVar()
name = "d%s/d%s" % (mvar.getName(), par.getName())
sens_var = casadi.MX.sym(name)
sens = ci.RealVariable(self, sens_var, ci.RealVariable.INTERNAL, ci.RealVariable.CONTINUOUS)
self.addVariable(sens)
# Compute derivatives
dfdx = {}
df0dx = {}
for vk in var_kinds:
dfdx[vk] = {}
df0dx[vk] = {}
for mvar in mvar_vectors[vk]:
mvar_var = mvar.getVar()
dfdx[vk][mvar.getName()] = N.array([casadi.jacobian(dae_eq, mvar_var) for dae_eq in dae])
df0dx[vk][mvar.getName()] = N.array([casadi.jacobian(init_eq, mvar_var) for init_eq in init])
# Add sensitivity differential equations
mx_zero = casadi.MX(0.)
for par in mvar_par:
for i in xrange(dae.numel()):
eq = mx_zero
for vk in var_kinds:
for mvar in mvar_vectors[vk]:
if vk == "dx":
name = "der(d%s/d%s)" % (mvar.getMyDifferentiatedVariable().getName(), par.getName())
else:
name = "d%s/d%s" % (mvar.getName(), par.getName())
sens_var = self.getVariable(name).getVar()
eq += dfdx[vk][mvar.getName()][i] * sens_var
eq += casadi.jacobian(dae[i], par.getVar())
sens_eq = ci.Equation(eq, mx_zero)
self.addDaeEquation(sens_eq)
# Add sensitivity initial equations
for par in mvar_par:
for i in xrange(init.numel()):
init_eq = mx_zero
for vk in var_kinds:
for mvar in mvar_vectors[vk]:
if vk == "dx":
name = "der(d%s/d%s)" % (mvar.getMyDifferentiatedVariable().getName(), par.getName())
else:
name = "d%s/d%s" % (mvar.getName(), par.getName())
sens_var = self.getVariable(name).getVar()
init_eq += df0dx[vk][mvar.getName()][i] * sens_var
init_eq += casadi.jacobian(init[i], par.getVar())
sens_init_eq = ci.Equation(init_eq, mx_zero)
self.addInitialEquation(sens_init_eq)