def _substitute_symbols(self, expr, variables, parameters):
if isinstance(expr, (int, float)):
return expr
for sym in symvar(expr):
[child, name] = self.symbol_dict[sym.getName()]
if name in child._variables:
expr = substitute(expr, sym, variables[child.label, name])
elif name in child._parameters:
expr = substitute(expr, sym, parameters[child.label, name])
return expr
def set_path(self, path, r2r=False):
"""Define an analytic expression of the geometric path.
Note: The path must be defined as a function of self.s[0].
Args:
path (list of SXMatrix): An expression of the geometric path as
a function of self.s[0]. Its dimension must equal self.sys.ny
r2r (boolean): Reparameterize path such that a rest to rest
transition is performed
Example:
>>> S = FlatSystem(2, 4)
>>> P = PathFollowing(S)
>>> P.set_path([P.s[0], P.s[0]])
"""
if isinstance(path, list):
path = cas.vertcat(path)
if r2r:
path = cas.substitute(path, self.s[0], self._r2r())
self.path[:, 0] = path
dot_s = cas.vertcat([self.s[1:], 0])
for i in range(1, self.sys.order + 1):
self.path[:, i] = cas.mul(
cas.jacobian(self.path[:, i - 1], self.s), dot_s)
def check_ss_feasibility(self, tol=1e-6):
"""Returns whether path is steady state feasible.
Args:
tol (double): tolerance for feasibility check (default = 1e-6)
Raises:
warning when bounds may be too strict
"""
for i, f in enumerate(self.constraints):
F = cas.substitute(f[0], self.sys.y, self.path)
F1 = cas.SXFunction([self.s], [F - f[1]])
F2 = cas.SXFunction([self.s], [-F + f[2]])
F1.init()
F2.init()
c1 = np.array([evalf(F1, np.hstack([s, [1e-50] * (self.sys.order)])).T
for s in self.prob['s']])
c2 = np.array([evalf(F2, np.hstack([s, [1e-50] * (self.sys.order)])).T
for s in self.prob['s']])
if np.any(c1 + tol < 0):
warnings.warn("lower bound of constraint %d may be to strict" % i)
return False
elif np.any(c2 + tol < 0):
warnings.warn("upper bound of constraint %d may be to strict" % i)
return False
return True
def sx_to_double(sx1,variables,values): val1 = casadi.substitute(sx1,variables,values) val1 = casadi.getValue(val1) import numpy if numpy.isnan(val1): print 'sx_to_double:Warning: casadi.getValue returned nan! \ Make sure "sx1" is fully specified once \ "values" have been substituted for "variables"' return val1
def _get_solution(self):
"""Get the solution from the solver output
Fills the dictionary self.sol with the information:
* 's': The optimal s as a function of time
* 't': The time vector
* 'states': Numerical values of the states defined in self.sys
TODO: perform accurate integration to determine time
TODO: Do exact interpolation
"""
solver = self.prob['solver']
N = self.options['N']
x_opt = np.array(solver.getOutput("x")).ravel()
b_opt = np.reshape(x_opt, (N + 1, -1), order='F')
self.sol['b'] = b_opt
# Determine time on a sufficiently fine spatial grid
s0 = np.linspace(0, 1, 1001)
delta = s0[1] - s0[0]
pieces = [lambda s, b=b_opt, ss=ss, j=j:
sum([bb * (s - ss) ** i / fact[i]
for i, bb in enumerate(b[j])])
for j, ss in enumerate(self.prob['s'][:-1])]
conds = lambda s0: [np.logical_and(self.prob['s'][i] <= s0,
s0 <= self.prob['s'][i+1])
for i in range(N)]
b0_opt = np.piecewise(s0, conds(s0), pieces)
b0_opt[b0_opt < 0] = 0
time = np.cumsum(np.hstack([0, 2 * delta / (np.sqrt(b0_opt[:-1]) +
np.sqrt(b0_opt[1:]))]))
# Resample to constant time-grid
t = np.arange(time[0], time[-1], self.options['Ts'])
st = np.interp(t, time, s0)
# Evaluate solution on equidistant time grid
b_opt = np.c_[[np.piecewise(st, conds(st), pieces, b=b_opt[:, i:])
for i in range(self.sys.order)]].T
st = np.matrix(st)
# Determine s and derivatives from b_opt
b, Ds = self._make_path()[1:]
Ds_f = cas.SXFunction([b], [Ds]) # derivatives of s wrt b
Ds_f.init()
s_opt = np.hstack((st.T, np.array([evalf(Ds_f, bb).toArray().ravel()
for bb in b_opt])))
self.sol['s'] = np.asarray(s_opt)
self.sol['t'] = t
# Evaluate the states
f = cas.SXFunction([self.s], [cas.substitute(cas.vertcat(self.sys.x.values()),
self.sys.y, self.path)])
f_val = np.array([evalf(f, s.T).toArray().ravel() for s in s_opt])
self.sol['states'] = dict([(k, f_val[:, i]) for i, k in
enumerate(self.sys.x.keys())])
def _check_for_lineq(self):
g = []
for con in self.global_constraints:
lb, ub = con[1], con[2]
g = vertcat(g, con[0] - lb)
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k, _ in enumerate(lb):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_global.values():
return False, None, None
par = struct_symMX(self.par_global_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.name() for s in symvar(b)]
dep_A = [s.name() for s in symvar(b)]
for name, sym in self.par_global.items():
if sym.name() in dep_b:
b = substitute(b, sym, par[name])
if sym.name() in dep_A:
A = substitute(A, sym, par[name])
A = Function('A', [par], [A]).expand()
b = Function('b', [par], [b]).expand()
return True, A, b
def _check_for_lineq(self):
g = []
for con in self.constraints:
lb, ub = con[1], con[2]
g = vertcat([g, con[0] - lb])
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k in range(len(lb)):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = child.get_variable(name, spline=False)
jj = jacobian(g, var)
jac = horzcat([jac, jj[:, ind]])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = child.get_variable(name, spline=False)
jj = jacobian(g, var)
jac = horzcat([jac, jj[:, ind]])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_i.values():
return False, None, None
par = struct_symMX(self.par_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.getName() for s in symvar(b)]
dep_A = [s.getName() for s in symvar(b)]
for name, sym in self.par_i.items():
if sym.getName() in dep_b:
b = substitute(b, sym, par[name])
if sym.getName() in dep_A:
A = substitute(A, sym, par[name])
A = MXFunction('A', [par], [A]).expand()
b = MXFunction('b', [par], [b]).expand()
return True, A, b
def _make_objective(self):
"""Construct objective function from the problem definition
Make time optimal objective function and add a regularization to
ensure a unique solution. When additional objective terms are defined
these are added to the objective as well.
TODO: Improve accuracy of integration
"""
order = self.sys.order
N = self.options['N']
b = self.prob['vars']
# ds = 1.0/(N+1)
obj = 2 * sum(np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0])))
# reg = sum(cas.sqrt((b[1:, order - 1] - b[:N, order - 1]) ** 2))
reg = sum((b[2:, order - 1] - 2 * b[1:N, order - 1] +
b[:(N - 1), order - 1]) ** 2)
for f in self.objective['Lagrange']:
path, bs = self._make_path()[0:2]
# S = np.arange(0, 1, 1.0/(N+1))
S = self.prob['s']
b = self.prob['vars']
L = cas.substitute(f, self.sys.y, path)
L = 2 * sum(cas.vertcat([
cas.substitute(
L,
cas.vertcat([self.s[0], bs]),
cas.vertcat([S[j], b[j, :].T])
) for j in range(1, N + 1)
])
* np.diff(self.prob['s']) / (cas.sqrt(b[:N, 0]) + cas.sqrt(b[1:, 0]))
)
# L = sum(cas.vertcat([cas.substitute(L, cas.vertcat([self.s[0],
# [bs[i] for i in range(0, bs.numel())]]),
# cas.vertcat([S[j], [b[j, i] for i in range(0, self.sys.order)]]))
# for j in range(0, N + 1)]))
obj = obj + L
self.prob['obj'] = obj + self.options['reg'] * reg
def _make_path(self):
"""Rewrite the path as a function of the optimization variables.
Substitutes the time derivatives of s in the expression of the path by
expressions that are function of b and its path derivatives by
repeatedly applying the chainrule
Returns:
* SXMatrix. The substituted path
* SXMatrix. b and the path derivatives
* SXMatrix. The derivatives of s as a function of b
"""
b = cas.ssym("b", self.sys.order)
db = cas.vertcat((b[1:], 0))
Ds = cas.SXMatrix.nan(self.sys.order) # Time derivatives of s
Ds[0] = cas.sqrt(b[0])
Ds[1] = b[1] / 2
# Apply chainrule for finding higher order derivatives
for i in range(1, self.sys.order - 1):
Ds[i + 1] = (cas.mul(cas.jacobian(Ds[i], b), db) * self.s[1] +
cas.jacobian(Ds[i], self.s[1]) * Ds[1])
Ds = cas.substitute(Ds, self.s[1], cas.sqrt(b[0]))
return cas.substitute(self.path, self.s[1:], Ds), b, Ds
def get_function(self, function_name):
if function_name in self.functions:
return self.functions[function_name]
try:
tree = self.root.classes[function_name]
except KeyError:
raise Exception('Unknown function {}'.format(function_name))
inputs = []
outputs = []
tmp = []
for s in tree.symbols.values():
src = self.get_mx(s)
if 'input' in s.prefixes:
inputs.append(src)
elif 'output' in s.prefixes:
outputs.append(src)
else:
tmp.append(src)
# Store current variable values
values = {}
for variable in inputs:
values[variable] = variable
# Process statements in order
for statement in tree.statements:
src = self.get_mx(statement)
for assignment in src:
[values[assignment.left]] = ca.substitute([assignment.right], list(values.keys()), list(values.values()))
output_expr = ca.substitute([values[output] for output in outputs], tmp, [values[t] for t in tmp])
func = ca.Function(tree.name, inputs, output_expr)
self.functions[function_name] = func
return func
def _make_constraints(self):
"""
Parse the constraints and put them in the correct format
"""
N = self.options['N']
con = self._ode(self.prob['vars'])
lb = np.alen(con) * [0]
ub = np.alen(con) * [0]
S = self.prob['s']
b = self.prob['vars']
path, bs = self._make_path()[0:2]
sbs = cas.vertcat([self.s[0], bs])
Sb = cas.horzcat([S, b]).T
for f in self.constraints:
F = cas.substitute(f[0], self.sys.y, path)
if f[3] is None:
F = cas.vertcat([cas.substitute(F, sbs, Sb[:, j])
for j in xrange(N + 1)])
Flb = f[1](S) if hasattr(f[1], '__call__') else [f[1]] * len(S)
Fub = f[2](S) if hasattr(f[2], '__call__') else [f[2]] * len(S)
con.append(F)
lb.extend(Flb)
ub.extend(Fub)
else:
F = cas.vertcat([cas.substitute(F, sbs, Sb[:, j])
for j in f[3]])
con.append(F)
lb.extend([f[1]])
ub.extend([f[2]])
if self.options['bc']:
con.append(b[0, 0])
lb.append(0)
ub.append(1e-50)
con.append(b[-1, 0])
lb.append(0)
ub.append(1e-50)
self.prob['con'] = [con, lb, ub]
def tangent_approx(f: SYM, x: SYM, a: SYM = None, assert_linear: bool = False) -> Dict[str, SYM]:
"""
Create a tangent approximation of a non-linear function f(x) about point a
using a block lower triangular solver
0 = f(x) = f(a) + J*x # taylor series about a (if f(x) linear in x, then globally valid)
J*x = -f(a) # solve for x
x = -J^{-1}f(a) # but inverse is slow, so we use solve
where J = df/dx
"""
# find f(a)
if a is None:
a = ca.DM.zeros(x.numel(), 1)
f_a = ca.substitute(f, x, a) # f(a)
J = ca.jacobian(f, x)
if assert_linear and ca.depends_on(J, x):
raise AssertionError('not linear')
# solve is smart enough to to convert to blt if necessary
return ca.solve(J, -f_a)
def update_expressions(self):
"""
Update OCP expressions using current parameter values.
"""
ocp_expressions = [self.ocp.initial, self.ocp.ode, self.ocp.alg,
self.ocp.path, self.ocp.point, self.ocp.mterm,
self.ocp.lterm]
parameters = casadi.vertcat([p.var() for p in self._parameters])
parameter_values = [p.getStart() for p in self._parameters]
[self.initial,
self.ode,
self.alg,
self.path,
self.point,
self.mterm,
self.lterm] = casadi.substitute(ocp_expressions, [parameters],
[parameter_values])
# Transform ODE RHS into residual
if self._casadi_blt:
self.ode = N.array(casadi.der(self.ocp.x)) - self.ode
def _get_solution(self):
solver = self.prob['solver']
N = self.options['N']
Nc = self.options['Nc']
x_opt = np.array(solver.getOutput("x")).ravel()
delta = np.diff(self.prob['s'])
b_opt = np.reshape(x_opt[:self.sys.order * (N + 1)], (N + 1, -1), order='F')
h_opt = np.reshape(x_opt[self.sys.order * (N + 1):], (Nc, -1), order='F')
time = np.cumsum(np.hstack([0, 2 * delta / (np.sqrt(b_opt[:-1, 0]) +
np.sqrt(b_opt[1:, 0]))]))
# Resample to constant time-grid
t = np.linspace(time[0], time[-1], self.options['Nt'])
b_opt = np.array([np.interp(t, time, b) for b in b_opt.T]).T
# Get s and derivatives from b_opt
s = np.interp(t, time, self.prob['s'])
b, Ds = self._make_path()[1:]
Ds_f = cas.SXFunction([b], [Ds]) # derivatives of s wrt b
Ds_f.init()
s_opt = np.vstack((s, np.vstack([evalf(Ds_f, bb).toArray().ravel()
for bb in b_opt]).T)).T
self.sol['s'] = s_opt
self.sol['h'] = h_opt
self.sol['b'] = b_opt
self.sol['t'] = t
# Evaluate the states
basisH = self._make_basis()
B = [np.dot(basisH(s_opt[:, 0]), h_opt)]
for i in range(1, self.h.size2()):
# B.append(np.matrix(np.dot(basisH.derivative(s_opt[:, 0], i), h_opt)))
Bi, p = basisH.derivative(i)
B.append(np.matrix(np.dot(np.dot(Bi(s_opt[:, 0]), p), h_opt)))
f = cas.SXFunction([cas.vertcat([self.s, cas.vec(self.h)])], [cas.substitute(self.sys.x.values(),
self.sys.y, self.path)])
f_val = np.array([evalf(f, s.T).toArray().ravel() for s in np.hstack((s_opt, np.hstack(B)))])
self.sol['states'] = dict([(k, f_val[:, i]) for i, k in
enumerate(self.sys.x.keys())])
def initialize(self, config_file=None):
"""
Initialize state vector with default values
:param config_file: Path to an initialization file.
"""
if config_file:
# TODO read start and stop time from config_file and call:
# self.setup_experiment(start,stop)
# for now, assume that setup_experiment was called beforehand
raise NotImplementedError
# Set values of parameters defined in the model into the state vector
for var in self.__pymoca_model.parameters:
# First check to see if parameter is already set (this allows child classes to override model defaults)
if np.isfinite(self.get_var(var.symbol.name())):
continue
# Also test to see if the value is constant
if isinstance(var.value, ca.MX) and not var.value.is_constant():
continue
# Try to extract the value
try:
# Extract the value as a python type
val = var.python_type(var.value)
except ValueError:
# var.value is a float NaN being cast to non-float
continue
else:
# If val is finite, we set it
if np.isfinite(val):
logger.debug(
'SimulationProblem: Setting parameter {} = {}'.format(
var.symbol.name(), val))
self.set_var(var.symbol.name(), val)
# Assemble initial residuals and set values from start attributes into the state vector
constrained_residuals = []
minimized_residuals = []
for var in itertools.chain(self.__pymoca_model.states,
self.__pymoca_model.alg_states):
var_name = var.symbol.name()
var_nominal = self.get_variable_nominal(var_name)
# Attempt to cast var.start to python type
mx_start = ca.MX(var.start)
if mx_start.is_constant():
# cast var.start to python type
start_val = var.python_type(mx_start.to_DM())
else:
# var.start is a symbolic expression with unknown value
start_val = None
if start_val == 0.0 and not var.fixed:
# To make initialization easier, we allow setting initial states by providing timeseries
# with names that match a symbol in the model. We only check for this matching if the start
# and fixed attributes were left as default
try:
start_val = self.initial_state()[var_name]
except KeyError:
pass
else:
# An initial state was found- add it to the constrained residuals
logger.debug(
'Initialize: Added {} = {} to initial equations (found matching timeseries).'
.format(var_name, start_val))
# Set var to be fixed
var.fixed = True
# Attempt to set start_val in the state vector. Default to zero if unknown.
try:
self.set_var(var_name,
start_val if start_val is not None else 0.0)
except KeyError:
logger.warning(
'Initialize: {} not found in state vector. Initial value of {} not set.'
.format(var_name, start_val))
# Add a residual for the difference between the state and its starting expression
start_expr = start_val if start_val is not None else var.start
if var.fixed:
# require residual = 0
constrained_residuals.append(
(var.symbol - start_expr) / var_nominal)
else:
# minimize residual
minimized_residuals.append(
(var.symbol - start_expr) / var_nominal)
# Default start var for ders is zero
for der_var in self.__mx['derivatives']:
self.set_var(der_var.name(), 0.0)
# Warn for nans in state vector (verify we didn't miss anything)
self.__warn_for_nans()
# Optionally encourage a steady-state initial condition
if getattr(self, 'encourage_steady_state_initial_conditions', False):
# add penalty for der(var) != 0.0
for d in self.__mx['derivatives']:
logger.debug('Added {} to the minimized residuals.'.format(
d.name()))
minimized_residuals.append(d)
# Make minimized_residuals into a single symbolic object
minimized_residual = ca.vertcat(*minimized_residuals)
# Assemble symbolics needed to make a function describing the initial condition of the model
# We constrain every entry in this MX to zero
equality_constraints = ca.vertcat(self.__dae_residual,
self.__initial_residual,
*constrained_residuals)
# The variables that need a mutually consistent initial condition
X = ca.vertcat(*self.__sym_list[:self.__states_end_index])
# Make a list of unscaled symbols and a list of their scaled equivalent
unscaled_symbols = []
scaled_symbols = []
for sym_name, nominal in self.__nominals.items():
# Add the symbol to the lists
symbol = self.__sym_dict[sym_name]
unscaled_symbols.append(symbol)
scaled_symbols.append(symbol * nominal)
# Make the lists symbolic
unscaled_symbols = ca.vertcat(*unscaled_symbols)
scaled_symbols = ca.vertcat(*scaled_symbols)
# Substitute unscaled terms for scaled terms
equality_constraints = ca.substitute(equality_constraints,
unscaled_symbols, scaled_symbols)
minimized_residual = ca.substitute(minimized_residual,
unscaled_symbols, scaled_symbols)
logger.debug('SimulationProblem: Initial Equations are ' +
str(equality_constraints))
logger.debug('SimulationProblem: Minimized Residuals are ' +
str(minimized_residual))
# State bounds can be symbolic, written in terms of parameters. After all
# parameter values are known, we evaluate the numeric values of bounds.
symbolic_bounds = ca.vertcat(*[
ca.horzcat(v.min, v.max) for v in itertools.chain(
self.__pymoca_model.states, self.__pymoca_model.alg_states,
self.__pymoca_model.der_states)
])
bound_evaluator = ca.Function('bound_evaluator',
self.__mx['parameters'],
[symbolic_bounds])
# Evaluate bounds using values of parameters
n_parameters = len(self.__mx['parameters'])
if n_parameters > 0:
[evaluated_bounds
] = bound_evaluator.call(self.__state_vector[-n_parameters:])
else:
[evaluated_bounds] = bound_evaluator.call([])
# Construct arrays of state bounds (used in the initialize() nlp, but not in __do_step rootfinder)
self.__lbx = evaluated_bounds[:, 0]
self.__ubx = evaluated_bounds[:, 1]
# Constrain model equation residuals to zero
lbg = np.zeros(equality_constraints.size1())
ubg = np.zeros(equality_constraints.size1())
# Construct objective function from the input residual
objective_function = ca.dot(minimized_residual, minimized_residual)
# Construct nlp and solver to find initial state using ipopt
parameters = ca.vertcat(*self.__mx['time'],
*self.__mx['constant_inputs'],
*self.__mx['parameters'])
nlp = {
'x': X,
'f': objective_function,
'g': equality_constraints,
'p': parameters
}
solver = ca.nlpsol('solver', 'ipopt', nlp, self.solver_options())
# Construct guess
guess = ca.vertcat(
*np.nan_to_num(self.__state_vector[:self.__states_end_index]))
# Find initial state
initial_state = solver(x0=guess,
lbx=self.__lbx,
ubx=self.__ubx,
lbg=lbg,
ubg=ubg,
p=self.__state_vector[self.__states_end_index:])
# If unsuccessful, stop.
return_status = solver.stats()['return_status']
if return_status not in {
'Solve_Succeeded', 'Solved_To_Acceptable_Level'
}:
raise Exception(
'Initialization Failed with return status "{}"'.format(
return_status))
# Update state vector with initial conditions
self.__state_vector[:self.__states_end_index] = initial_state[
'x'][:self.__states_end_index].T
# make a copy of the initialized initial state vector in case we want to run the model again
self.__initialized_state_vector = copy.deepcopy(self.__state_vector)
# Warn for nans in state vector after initialization
self.__warn_for_nans()
def __init__(self, **kwargs):
# Check arguments
assert ('model_folder' in kwargs)
# Log pymoca version
logger.debug("Using pymoca {}.".format(pymoca.__version__))
# Transfer model from the Modelica .mo file to CasADi using pymoca
if 'model_name' in kwargs:
model_name = kwargs['model_name']
else:
if hasattr(self, 'model_name'):
model_name = self.model_name
else:
model_name = self.__class__.__name__
# Load model from pymoca backend
self.__pymoca_model = pymoca.backends.casadi.api.transfer_model(
kwargs['model_folder'], model_name, self.compiler_options())
# Extract the CasADi MX variables used in the model
self.__mx = {}
self.__mx['time'] = [self.__pymoca_model.time]
self.__mx['states'] = [v.symbol for v in self.__pymoca_model.states]
self.__mx['derivatives'] = [
v.symbol for v in self.__pymoca_model.der_states
]
self.__mx['algebraics'] = [
v.symbol for v in self.__pymoca_model.alg_states
]
self.__mx['parameters'] = [
v.symbol for v in self.__pymoca_model.parameters
]
self.__mx['constant_inputs'] = []
self.__mx['lookup_tables'] = []
# TODO: implement delayed feedback
delayed_feedback_variables = []
for v in self.__pymoca_model.inputs:
if v.symbol.name() in delayed_feedback_variables:
# Delayed feedback variables are local to each ensemble, and
# therefore belong to the collection of algebraic variables,
# rather than to the control inputs.
self.__mx['algebraics'].append(v.symbol)
else:
if v.symbol.name() in kwargs.get('lookup_tables', []):
self.__mx['lookup_tables'].append(v.symbol)
else:
# All inputs are constant inputs
self.__mx['constant_inputs'].append(v.symbol)
# Log variables in debug mode
if logger.getEffectiveLevel() == logging.DEBUG:
logger.debug("SimulationProblem: Found states {}".format(', '.join(
[var.name() for var in self.__mx['states']])))
logger.debug("SimulationProblem: Found derivatives {}".format(
', '.join([var.name() for var in self.__mx['derivatives']])))
logger.debug("SimulationProblem: Found algebraics {}".format(
', '.join([var.name() for var in self.__mx['algebraics']])))
logger.debug("SimulationProblem: Found constant inputs {}".format(
', '.join([var.name()
for var in self.__mx['constant_inputs']])))
logger.debug("SimulationProblem: Found parameters {}".format(
', '.join([var.name() for var in self.__mx['parameters']])))
# Initialize an AliasDict for nominals and types
self.__nominals = AliasDict(self.alias_relation)
self.__python_types = AliasDict(self.alias_relation)
for v in itertools.chain(self.__pymoca_model.states,
self.__pymoca_model.alg_states,
self.__pymoca_model.inputs):
sym_name = v.symbol.name()
# Store the types in an AliasDict
self.__python_types[sym_name] = v.python_type
# If the nominal is 0.0 or 1.0 or -1.0, ignore: get_variable_nominal returns a default of 1.0
# TODO: handle nominal vectors (update() will need to load them)
if ca.MX(v.nominal).is_zero() or ca.MX(
v.nominal - 1).is_zero() or ca.MX(v.nominal + 1).is_zero():
continue
else:
if ca.MX(v.nominal).size1() != 1:
logger.error(
'Vector Nominals not supported yet. ({})'.format(
sym_name))
self.__nominals[sym_name] = ca.fabs(v.nominal)
if logger.getEffectiveLevel() == logging.DEBUG:
logger.debug(
"SimulationProblem: Setting nominal value for variable {} to {}"
.format(sym_name, self.__nominals[sym_name]))
# Initialize DAE and initial residuals
variable_lists = [
'states', 'der_states', 'alg_states', 'inputs', 'constants',
'parameters'
]
function_arguments = [self.__pymoca_model.time] + [
ca.veccat(*[
v.symbol for v in getattr(self.__pymoca_model, variable_list)
]) for variable_list in variable_lists
]
self.__dae_residual = self.__pymoca_model.dae_residual_function(
*function_arguments)
self.__initial_residual = self.__pymoca_model.initial_residual_function(
*function_arguments)
if self.__initial_residual is None:
self.__initial_residual = ca.MX()
# Construct state vector
self.__sym_list = self.__mx['states'] + self.__mx['algebraics'] + self.__mx['derivatives'] + \
self.__mx['time'] + self.__mx['constant_inputs'] + self.__mx['parameters']
self.__state_vector = np.full(len(self.__sym_list), np.nan)
# A very handy index
self.__states_end_index = len(self.__mx['states']) + \
len(self.__mx['algebraics']) + len(self.__mx['derivatives'])
# Construct a dict to look up symbols by name (or iterate over)
self.__sym_dict = OrderedDict(
((sym.name(), sym) for sym in self.__sym_list))
# Assemble some symbolics, including those needed for a backwards Euler derivative approximation
X = ca.vertcat(*self.__sym_list[:self.__states_end_index])
X_prev = ca.vertcat(*[
ca.MX.sym(sym.name() + '_prev')
for sym in self.__sym_list[:self.__states_end_index]
])
dt = ca.MX.sym("delta_t")
# Make a list of derivative approximations using backwards Euler formulation
derivative_approximation_residuals = []
for index, derivative_state in enumerate(self.__mx['derivatives']):
derivative_approximation_residuals.append(
derivative_state - (X[index] - X_prev[index]) / dt)
# Append residuals for derivative approximations
dae_residual = ca.vertcat(self.__dae_residual,
*derivative_approximation_residuals)
# TODO: implement lookup_tables
# Make a list of unscaled symbols and a list of their scaled equivalent
unscaled_symbols = []
scaled_symbols = []
for sym_name, nominal in self.__nominals.items():
index = self.__get_state_vector_index(sym_name)
# If the symbol is a state, Add the symbol to the lists
if index <= self.__states_end_index:
unscaled_symbols.append(X[index])
scaled_symbols.append(X[index] * nominal)
# Also scale previous states
unscaled_symbols.append(X_prev[index])
scaled_symbols.append(X_prev[index] * nominal)
# Substitute unscaled terms for scaled terms
dae_residual = ca.substitute(dae_residual,
ca.vertcat(*unscaled_symbols),
ca.vertcat(*scaled_symbols))
if logger.getEffectiveLevel() == logging.DEBUG:
logger.debug('SimulationProblem: DAE Residual is ' +
str(dae_residual))
if X.size1() != dae_residual.size1():
logger.error(
'Formulation Error: Number of states ({}) does not equal number of equations ({})'
.format(X.size1(), dae_residual.size1()))
# Construct function parameters
parameters = ca.vertcat(dt, X_prev,
*self.__sym_list[self.__states_end_index:])
# Construct a function res_vals that returns the numerical residuals of a numerical state
self.__res_vals = ca.Function("res_vals", [X, parameters],
[dae_residual])
# Use rootfinder() to make a function that takes a step forward in time by trying to zero res_vals()
options = {'nlpsol': 'ipopt', 'nlpsol_options': self.solver_options()}
self.__do_step = ca.rootfinder("next_state", "nlpsol", self.__res_vals,
options)
# Call parent class for default behaviour.
super().__init__()
def load_model(model_folder: str, model_name: str, compiler_options: Dict[str, str]) -> CachedModel:
"""
Loads a precompiled CasADi model into a CachedModel instance.
:param model_folder: Folder where the precompiled CasADi model is located.
:param model_name: Name of the model.
:param compiler_options: Dictionary of compiler options.
:returns: CachedModel instance.
"""
db_file = os.path.join(model_folder, model_name + ".pymoca_cache")
if compiler_options.get('mtime_check', True):
# Mtime check
cache_mtime = os.path.getmtime(db_file)
for folder in [model_folder] + compiler_options.get('library_folders', []):
for root, dir, files in os.walk(folder, followlinks=True):
for item in fnmatch.filter(files, "*.mo"):
filename = os.path.join(root, item)
if os.path.getmtime(filename) > cache_mtime:
raise InvalidCacheError("Cache out of date")
# Create empty model object
model = CachedModel()
# Load metadata
with open(db_file, 'rb') as f:
db = pickle.load(f)
if db['version'] != __version__:
raise InvalidCacheError('Cache generated for a different version of pymoca')
# Check compiler options. We ignore the library folders, as they have
# already been checked, and checking them will impede platform
# portability of the cache.
exclude_options = ['library_folders']
old_opts = {k: v for k, v in db['options'].items() if k not in exclude_options}
new_opts = {k: v for k, v in compiler_options.items() if k not in exclude_options}
if old_opts != new_opts:
raise InvalidCacheError('Cache generated for different compiler options')
# Pickles are platform independent, but dynamic libraries are not
if compiler_options.get('codegen', False):
if db['library_os'] != os.name:
raise InvalidCacheError('Cache generated for incompatible OS')
# Include references to the shared libraries
for o in ['dae_residual', 'initial_residual', 'variable_metadata', 'delay_arguments']:
if isinstance(db[o], str):
# Path to codegen'd library
f = ca.external(o, db[o])
else:
# Pickled CasADi Function; use as is
assert isinstance(db[o], ca.Function)
f = db[o]
setattr(model, '_' + o + '_function', f)
# Load variables per category
variables_with_metadata = ['states', 'alg_states', 'inputs', 'parameters', 'constants']
variable_dict = {}
for key in variables_with_metadata:
variables = getattr(model, key)
for i, d in enumerate(db[key]):
variable = Variable.from_dict(d)
variables.append(variable)
variable_dict[variable.symbol.name()] = variable
model.der_states = [Variable.from_dict(d) for d in db['der_states']]
model.outputs = db['outputs']
model.delay_states = db['delay_states']
model.alias_relation = db['alias_relation']
# Evaluate variable metadata:
parameter_vector = ca.veccat(*[v.symbol for v in model.parameters])
metadata = dict(zip(variables_with_metadata, model.variable_metadata_function(parameter_vector)))
independent_metadata = dict(zip(
variables_with_metadata,
(np.array(x) for x in model.variable_metadata_function(ca.veccat(*[np.nan for v in model.parameters])))))
for k, key in enumerate(variables_with_metadata):
m = db[key + "__metadata_dependent"]
for i, d in enumerate(db[key]):
variable = variable_dict[d['name']]
for j, tmp in enumerate(CASADI_ATTRIBUTES):
if m[i, j]:
setattr(variable, tmp, metadata[key][i, j])
else:
setattr(variable, tmp, independent_metadata[key][i, j])
# Evaluate delay arguments:
if model.delay_states:
args = [model.time,
ca.veccat(*model._symbols(model.states)),
ca.veccat(*model._symbols(model.der_states)),
ca.veccat(*model._symbols(model.alg_states)),
ca.veccat(*model._symbols(model.inputs)),
ca.veccat(*model._symbols(model.constants)),
ca.veccat(*model._symbols(model.parameters))]
delay_arguments_raw = model.delay_arguments_function(*args)
nan_args = [ca.repmat(np.nan, *arg.size()) for arg in args]
independent_delay_arguments_raw = model.delay_arguments_function(*nan_args)
delay_expressions_raw = delay_arguments_raw[::2]
delay_durations_raw = delay_arguments_raw[1::2]
independent_delay_durations_raw = independent_delay_arguments_raw[1::2]
assert 1 == len({len(delay_expressions_raw), len(delay_durations_raw),
len(independent_delay_durations_raw)})
all_symbols = [model.time,
*model._symbols(model.states),
*model._symbols(model.der_states),
*model._symbols(model.alg_states),
*model._symbols(model.inputs),
*model._symbols(model.constants),
*model._symbols(model.parameters)]
duration_dependencies = db['__delay_duration_dependent']
# Get rid of false dependency symbols not used in any delay
# durations. This significantly reduces the work the (slow)
# substitute() calls have to do later on.
actual_deps = sorted(set(np.array(duration_dependencies).ravel()))
actual_dep_symbols = [np.nan] * len(all_symbols)
for i in actual_deps:
actual_dep_symbols[i] = all_symbols[i]
delay_durations_simplified = ca.Function(
'replace_false_deps',
all_symbols,
delay_durations_raw).call(
actual_dep_symbols)
# Get rid of remaining hidden dependencies in the delay durations
for i, expr in enumerate(delay_expressions_raw):
if duration_dependencies[i]:
dur = delay_durations_simplified[i]
if len(duration_dependencies[i]) < len(actual_deps):
deps = set(ca.symvar(dur))
actual_deps = {all_symbols[j] for j in duration_dependencies[i]}
false_deps = deps - actual_deps
if false_deps:
[dur] = ca.substitute(
[dur],
list(false_deps),
[np.nan] * len(false_deps))
else:
# Already removed all false dependencies
pass
else:
dur = independent_delay_durations_raw[i]
model.delay_arguments.append(DelayArgument(expr, dur))
# Try to coerce parameters into their Python types
for p in model.parameters:
for attr in CASADI_ATTRIBUTES:
v = getattr(p, attr)
v_mx = ca.MX(v)
if v_mx.is_constant() and v_mx.is_regular():
setattr(p, attr, p.python_type(v))
# Done
return model
def exit_classDefinition(self, tree: etree._Element): # noqa: too-complex
dae = HybridDae()
dae.t = self.scope['time']
self.model[tree] = dae
# handle component declarations
for var_name, v in self.scope['var'].items():
variability = self.scope['prop'][var_name]['variability']
if variability == 'continuous':
if var_name in self.scope['states']:
dae.x = ca.vertcat(dae.x, v)
dae.dx = ca.vertcat(dae.dx, self.der(v))
else:
dae.y = ca.vertcat(dae.y, v)
elif variability == 'discrete':
dae.m = ca.vertcat(dae.m, v)
elif variability == 'parameter':
dae.p = ca.vertcat(dae.p, v)
elif variability == 'constant':
dae.p = ca.vertcat(dae.p, v)
else:
raise ValueError('unknown variability', variability)
for eq in self.scope['eqs']:
if isinstance(eq, self.sym):
dae.f_x = ca.vertcat(dae.f_x, eq)
# build reinit expression and discrete equations
dae.f_i = dae.x
dae.f_m = dae.m
for eq in self.scope['when_eqs']:
w = eq['cond']
for then_eq in eq['then']:
if isinstance(then_eq, tuple):
if then_eq[0] == 'reinit':
sub_var = then_eq[1]
sub_expr = ca.if_else(self.edge(w), then_eq[2],
sub_var)
dae.f_i = ca.substitute(dae.f_i, sub_var, sub_expr)
elif isinstance(then_eq, self.sym):
# this is a discrete variable assignment
# so it should be a casadi subtraction y = x
assert then_eq.is_op(ca.OP_SUB) and then_eq.n_dep() == 2
sub_var = then_eq.dep(0)
sub_expr = ca.if_else(self.edge(w), then_eq.dep(1),
sub_var)
dae.f_m = ca.substitute(dae.f_m, sub_var, sub_expr)
dae.t = self.scope['time']
dae.prop.update(self.scope['prop'])
c_dict = self.scope['c']
for k in c_dict.keys():
dae.c = ca.vertcat(dae.c, k)
dae.pre_c = ca.vertcat(dae.pre_c, self.pre_cond(k))
dae.f_c = ca.vertcat(dae.f_c, c_dict[k])
for left, right in [('f_c', 'c'), ('c', 'pre_c'), ('dx', 'x'),
('f_m', 'm')]:
vl = getattr(dae, left)
vr = getattr(dae, right)
if vl.shape != vr.shape:
raise ValueError('{:s} and {:s} must have the same shape:'
'\n{:s}: {:s}\t{:s}: {:s}'.format(
left, right, left, str(dae.f_m), right,
str(dae.m)))
dae.ng = ca.vertcat(*self.scope['ng'])
dae.nu = ca.vertcat(*self.scope['nu'])
n_eq = dae.f_x.shape[0] + dae.f_m.shape[0]
n_var = dae.x.shape[0] + dae.m.shape[0] + dae.y.shape[0]
if n_eq != n_var:
raise ValueError('must have equal number of equations '
'{:d} and unknowns {:d}\n:{:s}'.format(
n_eq, n_var, str(dae)))
self.scope_stack.pop()
def _construct_upd_z_nlp(self):
# construct variables
self._var_struct_updz = struct([entry('z_i', struct=self.q_i_struct),
entry('z_ij', struct=self.q_ij_struct)])
var = struct_symMX(self._var_struct_updz)
z_i = self.q_i_struct(var['z_i'])
z_ij = self.q_ij_struct(var['z_ij'])
# construct parameters
self._par_struct_updz = struct([entry('x_i', struct=self.q_i_struct),
entry('x_j', struct=self.q_ij_struct),
entry('l_i', struct=self.q_i_struct),
entry('l_ij', struct=self.q_ij_struct),
entry('t'), entry('T'), entry('rho'),
entry('par', struct=self.par_struct)])
par = struct_symMX(self._par_struct_updz)
x_i, x_j = self.q_i_struct(par['x_i']), self.q_ij_struct(par['x_j'])
l_i, l_ij = self.q_i_struct(par['l_i']), self.q_ij_struct(par['l_ij'])
t, T, rho = par['t'], par['T'], par['rho']
t0 = t/T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
# construct constraints
constraints, lb, ub = [], [], []
for con in self.constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_i[label, name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_i.items():
if s.getName() == sym.getName():
c = substitute(c, sym, par['par', name])
constraints.append(c)
lb.append(con[1])
ub.append(con[2])
self.lb_updz, self.ub_updz = lb, ub
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label, name]
z = z_i[child.label, name]
l = l_i[child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name in q_ij.keys():
x = x_j[str(nghb), child.label, name]
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
# construct problem
prob, compile_time = self.father.create_nlp(var, par, obj,
constraints, self.options)
self.problem_upd_z = prob
def convert_expr_from_time_to_tau(self, expr, t_k, t_kp1):
t = self.t
tau = self.tau
h = t_kp1 - t_k
return substitute(expr, t, tau * h + t_k)
def _nlp_solver(self, initial_cond, final_term_cost, trajectory_target, constraints):
'''Setting up the non linear problem settings, manually discretizing each time step'''
# Start with an empty NLP
w = [] # free variables vector
w_g = [] # initial guess
lbw = [] # lower bounds of inputs
ubw = [] # upper bounds of inputs
J = 0 # initial value of cost func
g = [] # constraints vector
lbg = [] # lower bounds of constraints
ubg = [] # upper bound of constraints
# Defining a new time variable because we need MX here
self.t = cs.MX.sym("t", 1)
self.traj, self.traj_dot = self.format_trajectory(trajectory_target, self.t)
# With SEA we have double the state variable and double the initial condition.
# We assume to start on the equilibrium
if self.sea and self.SEAdynamics:
initial_cond = initial_cond * 2
# Finally, we integrate all over the timeframe
dt = self.T / self.N
# Defining the variables needed for the integration
Xk = cs.MX.sym('X0', self.num_state_var * 2)
# Adding it to the vector that has to be optimized
w += [Xk]
# Setting the initial condition
w_g += initial_cond
lbw += initial_cond
ubw += initial_cond
for k in range(self.N):
# Variable for the control
Uk = cs.MX.sym('U_' + str(k), self.num_joints) # generate the k-th control command, nx1 dimension
w += [Uk]
self.add_input_constraints(w_g, lbw, ubw)
# Integrate till the end of the interval
Fk = self.F(x0=Xk, p=Uk, time=dt * k) # This is the actual integration!
Xk_next = Fk['xf']
J = J + Fk['qf']
# Add custom constraints
if constraints is not None:
self.add_custom_constraints(g, lbg, ubg, Xk, Uk, dt * k, constraints)
# Defining a new state variable
Xk = cs.MX.sym('X'+str(k+1), self.num_state_var * 2)
w += [Xk]
self.add_state_constraints(lbw, ubw, w_g, initial_cond)
# Imposing equality constraint
g += [Xk_next - Xk] # Imposing the state variables to work under the differential equation constraints
lbg += [0] * (2 * self.num_state_var)
ubg += [0] * (2 * self.num_state_var)
# Setting constraints on the last timestep
if constraints is not None:
self.add_custom_constraints(g, lbg, ubg, Xk, None, self.T, constraints)
# If we are optimizing for minimum T, we set its boundaries
if isinstance(self.T, cs.casadi.MX):
w += [self.T]
w_g += [1.0]
lbw += [0.05]
ubw += [float('inf')]
# Add a final term cost. If not specified is 0.
if final_term_cost is not None:
if self.sea and self.SEAdynamics:
Q_dd = self.q_ddot_val(Xk[0:self.num_joints], Xk[self.num_joints:2 * self.num_joints],
Xk[2*self.num_joints:3*self.num_joints], Xk[3*self.num_joints:4*self.num_joints], np.array([0]*self.num_joints))
else:
Q_dd = self.q_ddot_val(Xk[0:self.num_joints], Xk[self.num_joints:2 * self.num_joints], np.array([0]*self.num_joints))
EE_pos = self.ee_pos(Xk[0:self.num_joints])
J = J + final_term_cost(Xk[0:self.num_joints] - cs.substitute(self.traj, self.t, self.T),
Xk[self.num_joints:2*self.num_joints] - cs.substitute(self.traj_dot, self.t, self.T),
Q_dd,
EE_pos,
Uk)
# Adding constraint on final timestep
if self.final_constraints is not None:
self.add_custom_constraints(g, lbg, ubg, Xk, None, self.T, self.final_constraints, final=True)
# Define the problem to be solved
problem = {'f': J, 'x': cs.vertcat(*w), 'g': cs.vertcat(*g)}
# NLP solver options
opts = {}
opts["ipopt"] = {'max_iter': self.max_iter}
# Define the solver and add boundary conditions
solver = cs.nlpsol('solver', 'ipopt', problem, opts)
self.solver = solver(x0=w_g, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
def evalX(self, x, K):
[X] = cs.substitute([self._X], [self._x, self._K], [x, K])
return cs.evalf(X)
def optim(x):
#for x_i, value in zip(x, paramsFlat):
#print '%s: %s' % (x_i, value)
ret = casadi.substitute(ll, params, casadi.vertcat(*x))
return float(ret)
a = casadi.SX.sym
#def __init__(self, mu, v, b, c, lamb, gamma, alpha, beta, omega, d, e):
asVals=False
params1 = Params('GARCH', 1, 2, 2, 0, 0, .0, 0, .10, .8, .001/STEPS, 2.1, 2.1, asVals=asVals)
params2 = Params('GARCH', 2, 2, 2, 0, 0, .0, 0, .06, .92, .001/STEPS, 2.1, 6.8, asVals=True)
with Timer('ll calc'):
ll, (p_1, p_2, h_1, h_2) = loglikilihood(r, params1, params2)
if not asVals:
with Timer('subs model'):
ll = params1.subsForModel(ll)
#ll = params2.subsForModel(ll)
params = casadi.vertcat(params1.vertcat) #, params2.vertcat)
param_vals = casadi.vertcat(params1.vertcatvals) #, params2.vertcatvals)
with Timer('subs ll'):
print casadi.substitute(ll, params, param_vals)
#with Timer('calc hess'):
# hess = casadi.hessian(ll, params)
#with Timer('subs hess'):
# hessret = casadi.substitute(hess[0], params, param_vals)
# hessret2 = casadi.substitute(hess[1], params, param_vals)
with Timer('calc jacob'):
jacob = casadi.jacobian(ll, params)
with Timer('subs jacob'):
jacobret = casadi.substitute(jacob, params, param_vals)
#import pdb; pdb.set_trace()
bounds = []
paramsFlat = [params[x] for x in xrange(params.shape[0])]
for count, value in enumerate(paramsFlat):
bounds.append(params1.bound(value))
def rpm_sweep(prop_name, v0, kv0, i0_motor0, V_inf0, R_motor0):
prop_data = prop_db[prop_name]
key0 = list(prop_data['dynamic'].keys())[0]
my_prop = prop_data['dynamic'][key0]['data']
CT_lut = ca.interpolant('CT', 'bspline', [my_prop.index], my_prop.CT)
CP_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.CP)
eta_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.eta)
eta_prop_lut = ca.interpolant('eta', 'bspline', [my_prop.index],
my_prop.eta)
Q_prop_lut = ca.substitute(Q_prop, CP, CP_lut(J))
T_prop_lut = ca.substitute(T_prop, CT, CT_lut(J))
states = ca.vertcat(rpm_prop)
params = ca.vertcat(rho, r_prop_in, v_batt, Kv_motor, i0_motor, V_inf,
R_motor)
p0 = [1.225, prop_data['D'] / 2, v0, kv0, i0_motor0, V_inf0, R_motor0]
f_Q_prop = ca.Function('Q_prop', [states, params], [Q_prop_lut])
f_Q_motor = ca.Function('Q_motor', [states, params], [Q_motor])
f_T_prop = ca.Function('T_prop', [states, params], [T_prop_lut])
f_eta_motor = ca.Function('eta_motor', [states, params], [eta_motor])
f_eta_prop = ca.Function('eta_prop', [states, params], [eta_prop_lut(J)])
f_eta = ca.Function('eta', [states, params], [eta_prop_lut(J) * eta_motor])
rpm = np.array([np.linspace(0, 4000, 1000)])
plt.figure()
plt.subplot(311)
plt.plot(rpm.T, f_Q_motor(rpm, p0).T, label='motor')
plt.plot(rpm.T, f_Q_prop(rpm, p0).T, label='prop')
plt.title('prop motor matching')
plt.legend()
plt.ylabel('torque, N-m')
plt.grid(True)
plt.subplot(312)
plt.plot(rpm.T, f_T_prop(rpm, p0).T, label='prop')
plt.legend()
plt.ylabel('thrust, N')
plt.grid(True)
plt.subplot(313)
plt.plot(rpm.T, f_eta_motor(rpm, p0).T, label='motor')
plt.plot(rpm.T, f_eta_prop(rpm, p0).T, label='prop')
plt.plot(rpm.T, f_eta(rpm, p0).T, label='total')
plt.xlabel('prop angular velocity, rpm')
plt.ylabel('efficiency')
plt.grid(True)
plt.legend()
plt.gca().set_ylim(0, 1)
plt.show()
## scipy.sparse.csc_matrix is said to be better by casadi author
f_eta_prop_array = np.array(f_eta_prop(rpm, p0))[0]
max_eta_prop = np.nanmax(f_eta_prop_array)
index = np.where(f_eta_prop_array == max_eta_prop)
omega = rpm[0][index][0]
T = np.array(f_T_prop(omega, p0))[0]
Q = np.array(f_Q_prop(omega, p0))[0]
display(Math(r'\eta_{prop, max} = ' + str(max_eta_prop) \
+ r'\ at\ \Omega =\ ' + str(omega) + r'\ \frac{rev}{min}'))
## TODO get ideal motor properties in a dictionary and store them, including motor eta
data = {
'prop_name': prop_name,
'max_eta_prop': max_eta_prop,
'omega': omega,
'T': T,
'Q': Q
}
store(data)
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 plotNLP(self, x0, DTplot=10, additionalplottags=None):
plotH = []
plotAdd = []
plotT = []
plotTags = self.plotTags
ocp = self.ocp
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
stateScaling = self.stateScaling
DT = self.DT
Z = x0 / self.stateScaling
odeF = self.odeF
xOpt = self.xOpt
uOpt = self.uOpt
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", DTplot)
G.init()
pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
if additionalplottags is None:
Config = ConfigParser.ConfigParser()
Config.read('config.ini')
additionalplottags = Config.get('MultipleShooting',
'additionalplottags')
additionalplottags = additionalplottags.split(',')
self.additionalplottags = additionalplottags
addTagScale = C.vertcat([ocp.nominal(pv) for pv in additionalplottags])
tagsFsx = C.substitute(
C.vertcat([ocp.beq(sv) for sv in additionalplottags]),
C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
tagsF = C.SXFunction(pathIn, [tagsFsx])
tagsF.init()
tagsF.setInput(xOpt, 'x')
tagsF.setInput([], 'z')
tagsF.setInput(uOpt, 'p')
tagsF.setInput(0, 't')
tagsF.evaluate()
addTagsOpt = tagsF.getOutput()
for i in range(len(additionalplottags)):
self.optimalValues[additionalplottags[i]] = addTagsOpt[i]
sxPlotTags = C.vertcat([ocp.beq(tag) for tag in plotTags])
pathS = C.substitute(
sxPlotTags, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
pathF = C.SXFunction(pathIn, [pathS, tagsFsx])
pathF.init()
nPlot = NP.int(NP.round(self.DT / DTplot))
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):
try:
for jp in range(nPlot):
#Z,QF,zf = C.integratorOut(G(C.integratorIn(x0=Z,p=self.USOL[k],z0=zf)),"xf","qf","zf")
G.setInput(Z, 'x0')
G.setInput(self.USOL[k], 'p')
G.evaluate()
Z = G.getOutput('xf')
QF = G.getOutput('qf')
zf = G.getOutput('zf')
pathF.setInput(Z, 'x')
pathF.setInput(zf, 'z')
pathF.setInput(self.USOL[k], 'p')
t = k * DT + jp * DTplot
pathF.setInput(t, 't')
pathF.evaluate()
p = pathF.getOutput(0)
pAdd = pathF.getOutput(1)
plotH.append(p)
plotAdd.append(pAdd)
plotT.append(t)
Z = self.XSOL[k]
except:
print 'something bad happened', sys.exc_info()[0]
break
plotDic = {}
plotDic['t'] = plotT
for si in range(len(plotTags)):
plotDic[plotTags[si]] = [i[si] for i in plotH]
for si in range(len(additionalplottags)):
plotDic[additionalplottags[si]] = [i[si] for i in plotAdd]
self.plotDic = plotDic
def multiple_shooting(model,
problem,
time_vector,
options=None,
unpack_objective=False):
opts = {}
opts['integration_opts'] = None
opts['leq0_only'] = False
opts['with_initial_cnss'] = True
opts['with_final_cnss'] = True
if not options is None:
for k in options.keys():
if k in opts:
opts[k] = options[k]
else:
warn('Option not recognized : ' + k)
nSteps = time_vector.numel() - 1
tsym = model.t
dtsym = model.dt
# collect the parameters
if model.p != []:
pnum = len(model.p)
pindxs = list(range(pnum))
psyms = cs.vertcat(*[v.sym for v in model.p])
pdata = []
for k in range(pnum):
pdata.append({'lob': cs.repmat(cs.reshape(model.p[k].lob,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.p[k].lob).numel())))[:nSteps+1],\
'upb': cs.repmat(cs.reshape(model.p[k].upb,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.p[k].upb).numel())))[:nSteps+1],\
'val': cs.repmat(cs.reshape(model.p[k].val,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.p[k].val).numel())))[:nSteps+1],\
'lbl': model.p[k].lbl+"<p>",\
'nme': model.p[k].nme
})
plbl = ['<ocp_p>'] * pnum
for i in range(pnum):
if not model.p[i].lbl is None: plbl[i] += model.p[i].lbl
else:
pdata = []
pnum = 0
pindxs = []
psyms = cs.MX()
plbl = []
# collect the states
if model.x != []:
xnum = len(model.x)
xindxs = list(range(pnum, pnum + xnum))
xsyms = cs.vertcat(*[v.sym for v in model.x])
xdata = []
for k in range(xnum):
xdata.append({'lob': cs.repmat(cs.reshape(model.x[k].lob,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.x[k].lob).numel())))[:nSteps+1],\
'upb': cs.repmat(cs.reshape(model.x[k].upb,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.x[k].upb).numel())))[:nSteps+1],\
'val': cs.repmat(cs.reshape(model.x[k].val,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.x[k].val).numel())))[:nSteps+1],\
'lbl': model.x[k].lbl+"<x>",\
'nme': model.x[k].nme
})
xlbl = ['<ocp_x>'] * xnum
for i in range(xnum):
if not model.x[i].lbl is None: xlbl[i] += model.x[i].lbl
else:
xdata = []
xnum = 0
xindxs = []
xsyms = cs.MX()
xlbl = []
# collect the dicrete transition states
if model.y != []:
ynum = len(model.y)
yindxs = list(range(pnum + xnum, pnum + xnum + ynum))
ysyms = cs.vertcat(*[v.sym for v in model.y])
ydata = []
for k in range(ynum):
ydata.append({'lob': cs.repmat(cs.reshape(model.y[k].lob,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.y[k].lob).numel())))[:nSteps+1],\
'upb': cs.repmat(cs.reshape(model.y[k].upb,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.y[k].upb).numel())))[:nSteps+1],\
'val': cs.repmat(cs.reshape(model.y[k].val,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.y[k].val).numel())))[:nSteps+1],\
'lbl': model.y[k].lbl+"<y>",\
'nme': model.y[k].nme
})
ylbl = ['<ocp_y>'] * ynum
for i in range(ynum):
if not model.y[i].lbl is None: ylbl[i] += model.y[i].lbl
else:
ydata = []
ynum = 0
yindxs = []
ysyms = cs.MX()
ylbl = []
# collect the slack variables
if model.a != []:
anum = len(model.a)
aindxs = list(range(pnum + xnum + ynum, pnum + xnum + ynum + anum))
asyms = cs.vertcat(*[v.sym for v in model.a])
adata = []
for k in range(anum):
adata.append({'lob': cs.repmat(cs.reshape(model.a[k].lob,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.a[k].lob).numel())))[:nSteps+1],\
'upb': cs.repmat(cs.reshape(model.a[k].upb,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.a[k].upb).numel())))[:nSteps+1],\
'val': cs.repmat(cs.reshape(model.a[k].val,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.a[k].val).numel())))[:nSteps+1],\
'lbl': model.a[k].lbl+"<a>",\
'nme': model.a[k].nme
})
albl = ['<ocp_a>'] * anum
for i in range(anum):
if not model.a[i].lbl is None: albl[i] += model.a[i].lbl
else:
adata = []
anum = 0
aindxs = []
asyms = cs.MX()
albl = []
# collect the continuous controls
if model.u != []:
unum = len(model.u)
uindxs = list(
range(pnum + xnum + ynum + anum, pnum + xnum + ynum + anum + unum))
usyms = cs.vertcat(*[v.sym for v in model.u])
udata = []
for k in range(unum):
udata.append({'lob': cs.repmat(cs.reshape(model.u[k].lob,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.u[k].lob).numel())))[:nSteps+1],\
'upb': cs.repmat(cs.reshape(model.u[k].upb,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.u[k].upb).numel())))[:nSteps+1],\
'val': cs.repmat(cs.reshape(model.u[k].val,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.u[k].val).numel())))[:nSteps+1],\
'lbl': model.u[k].lbl+"<u>",\
'nme': model.u[k].nme
})
ulbl = ['<ocp_u>'] * unum
for i in range(unum):
if not model.u[i].lbl is None: ulbl[i] += model.u[i].lbl
else:
udata = []
unum = 0
uindxs = []
usyms = []
ulbl = []
# collect discrete controls
if model.v != []:
vnum = len(model.v)
vindxs = list(
range(pnum + xnum + ynum + anum + unum,
pnum + xnum + ynum + anum + unum + vnum))
vsyms = cs.vertcat(*[v.sym for v in model.v])
vdata = []
for k in range(vnum):
vdata.append({'lob': cs.repmat(cs.reshape(model.v[k].lob,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.v[k].lob).numel())))[:nSteps+1],\
'upb': cs.repmat(cs.reshape(model.v[k].upb,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.v[k].upb).numel())))[:nSteps+1],\
'val': cs.repmat(cs.reshape(model.v[k].val,1,-1),1,int(cs.ceil((nSteps+1)/cs.DM(model.v[k].val).numel())))[:nSteps+1],\
'lbl': model.v[k].lbl+"<v>",\
'nme': model.v[k].nme
})
vlbl = ['<ocp_v>'] * vnum
for i in range(vnum):
if not model.v[i].lbl is None: vlbl[i] += model.v[i].lbl
else:
vdata = []
vnum = 0
vindxs = []
vsyms = []
vlbl = []
# collect the external inputs
if model.i != []:
isyms = cs.vertcat(*[v.sym for v in model.i])
idata = cs.vertcat(*[v.val.T for v in model.i])
else:
isyms = cs.DM()
idata = cs.DM(0, nSteps + 1)
# create the set of variables for multiple shooting
syms = cs.vertcat(psyms, xsyms, ysyms, asyms, usyms, vsyms)
vardata = pdata + xdata + ydata + adata + udata + vdata
numVars = len(vardata)
# construct the problem variables
problem.var['sym'] = cs.MX.sym(
'V', nSteps * numVars + pnum + xnum + ynum + anum, 1)
problem.var['lob'] = cs.DM(
cs.reshape(cs.vertcat(*[vardata[k]['lob'] for k in range(numVars)]),
-1, 1)[:nSteps * numVars + anum + ynum + xnum + pnum])
problem.var['upb'] = cs.DM(
cs.reshape(cs.vertcat(*[vardata[k]['upb'] for k in range(numVars)]),
-1, 1)[:nSteps * numVars + anum + ynum + xnum + pnum])
problem.var['val'] = cs.DM(
cs.reshape(cs.vertcat(*[vardata[k]['val'] for k in range(numVars)]),
-1, 1)[:nSteps * numVars + anum + ynum + xnum + pnum])
problem.var['dsc'] = ([False] *
(pnum + xnum + ynum + anum + unum) + [True] * vnum
) * nSteps + [False] * (pnum + xnum + ynum + anum)
problem.var['nme'] = [
v['nme'] for v in pdata + xdata + ydata + adata + udata + vdata
] * nSteps + [v['nme'] for v in pdata + xdata + ydata + adata]
problem.var['lbl'] = [
v['lbl'] for v in pdata + xdata + ydata + adata + udata + vdata
] * nSteps + [v['lbl'] for v in pdata + xdata + ydata + adata]
problem.var['lam'] = None
# collect data for multiple shooting
shooting = {}
shooting['t'] = time_vector.T
shooting['dt'] = shooting['t'][1:] - shooting['t'][:-1]
shooting['i'] = idata
shooting['p'] = cs.reshape(
problem.var['sym'][[
numVars * i + j for i in range(nSteps + 1) for j in pindxs
]], (pnum, nSteps + 1))
shooting['x'] = cs.reshape(
problem.var['sym'][[
numVars * i + j for i in range(nSteps + 1) for j in xindxs
]], (xnum, nSteps + 1))
shooting['y'] = cs.reshape(
problem.var['sym'][[
numVars * i + j for i in range(nSteps + 1) for j in yindxs
]], (ynum, nSteps + 1))
shooting['a'] = cs.reshape(
problem.var['sym'][[
numVars * i + j for i in range(nSteps + 1) for j in aindxs
]], (anum, nSteps + 1))
shooting['u'] = cs.reshape(
problem.var['sym'][[
numVars * i + j for i in range(nSteps) for j in uindxs
]], (unum, nSteps))
shooting['v'] = cs.reshape(
problem.var['sym'][[
numVars * i + j for i in range(nSteps) for j in vindxs
]], (vnum, nSteps))
# collect the constraints
labels_tmp = []
problem.cns = {
'exp': cs.MX(),
'lob': cs.DM(),
'upb': cs.DM(),
'lbl': [],
'lam': None
}
# add path constraints
if model.pcns != []:
if not opts['leq0_only']:
pcnsf = cs.Function('pcns', [
cs.vertcat(
tsym,
psyms,
xsyms,
ysyms,
asyms,
isyms,
usyms,
vsyms,
dtsym,
)
], [cs.vertcat(*[c.exp for c in model.pcns])])
pcnsf = pcnsf.map(nSteps)
problem.cns['exp'] = cs.vertcat(
problem.cns['exp'],
pcnsf(
cs.vertcat(shooting['t'][:-1], shooting['x'][:, :-1],
shooting['y'][:, :-1], shooting['a'][:, :-1],
shooting['p'][:, :-1], shooting['i'][:, :-1],
shooting['u'], shooting['v'], shooting['dt'])))
problem.cns['lob'] = cs.vertcat(
problem.cns['lob'],
cs.repmat(cs.vertcat(*[c.lob for c in model.pcns]),
(1, nSteps)))
problem.cns['upb'] = cs.vertcat(
problem.cns['upb'],
cs.repmat(cs.vertcat(*[c.upb for c in model.pcns]),
(1, nSteps)))
labels_tmp += [
model.pcns[i].lbl + '<ocp_pc' + str(i) + '>'
for i in range(len(model.pcns))
]
else:
pcnsexp = cs.vertcat(*([c.lob - c.exp for c in model.pcns if c.lob > -cs.DM.inf()] +\
[c.exp - c.upb for c in model.pcns if c.upb < cs.DM.inf()]))
pcnsf = cs.Function('pcns', [
cs.vertcat(tsym, psyms, xsyms, ysyms, asyms, isyms, usyms,
vsyms, dtsym)
], [pcnsexp])
pcnsf = pcnsf.map(nSteps)
problem.cns['exp'] = cs.vertcat(
problem.cns['exp'],
pcnsf(
cs.vertcat(shooting['t'][:-1], shooting['x'][:, :-1],
shooting['y'][:, :-1], shooting['a'][:, :-1],
shooting['p'][:, :-1], shooting['i'][:, :-1],
shooting['u'], shooting['v'], shooting['dt'])))
problem.cns['lob'] = cs.vertcat(
problem.cns['lob'], -cs.DM.inf(pcnsexp.shape[0], nSteps))
problem.cns['upb'] = cs.vertcat(
problem.cns['upb'], cs.DM.zeros(pcnsexp.shape[0], nSteps))
labels_tmp += [model.pcns[i].lbl + '<ocp_pc'+str(i)+'>' for i,c in enumerate(model.pcns) if c.lob > -cs.DM.inf()] +\
[model.pcns[i].lbl + '<ocp_pc'+str(i)+'>' for i,c in enumerate(model.pcns) if c.upb < cs.DM.inf()]
# parameters are treated like states with zero dynamics
if model.p != []:
# add constraints
problem.cns['exp'] = cs.vertcat(
problem.cns['exp'], shooting['p'][:, :-1] - shooting['p'][:, 1:])
problem.cns['lob'] = cs.vertcat(problem.cns['lob'],
cs.DM.zeros(pnum, nSteps))
problem.cns['upb'] = cs.vertcat(problem.cns['upb'],
cs.DM.zeros(pnum, nSteps))
# collect shooting labels
labels_tmp += ['<ocp_par' + str(i) + '>' for i in range(pnum)]
# dynamic constraints
if model.ode != []:
# define integrator
intg = integrate_ode(
xsyms, cs.vertcat(tsym, ysyms, asyms, psyms, isyms, usyms, vsyms),
model.ode, dtsym, opts['integration_opts'])
# evaluate integration
intg = intg.map(nSteps)
intgval = intg.call({
'x0':
shooting['x'][:, :-1],
'p':
cs.vertcat(shooting['t'][:-1], shooting['y'][:, :-1],
shooting['a'][:, :-1], shooting['p'][:, :-1],
shooting['i'][:, :-1], shooting['u'], shooting['v'],
shooting['dt'])
})
# add integration constraints
if not opts['leq0_only']:
problem.cns['exp'] = cs.vertcat(
problem.cns['exp'], shooting['x'][:, 1:] - intgval['xf'])
problem.cns['lob'] = cs.vertcat(problem.cns['lob'],
cs.DM.zeros(xnum, nSteps))
problem.cns['upb'] = cs.vertcat(problem.cns['upb'],
cs.DM.zeros(xnum, nSteps))
# classify the resulting constraints
tmp_order_a = get_order(cs.vertcat(*model.ode), [
syms[i] for i in range(pnum + xnum + ynum + anum + unum + vnum)
]) # global order
tmp_order_c = get_order(
cs.vertcat(*model.ode),
[syms[i] for i in range(pnum + xnum + ynum + anum + unum)
]) # order considering the continuous variables only
labels_tmp += [
'<ocp_int' + str(i) + '>' + '<semi_cvx>' *
int(tmp_order_a[i] > 1) * int(tmp_order_c[i] <= 1) +
'<non-convex>' * int(tmp_order_c[i] > 1) for i in range(xnum)
]
else:
problem.cns['exp'] = cs.vertcat(
problem.cns['exp'], shooting['x'][:, 1:] - intgval['xf'],
intgval['xf'] - shooting['x'][:, 1:])
problem.cns['lob'] = cs.vertcat(problem.cns['lob'],
-cs.DM.inf(2 * xnum, nSteps))
problem.cns['upb'] = cs.vertcat(problem.cns['upb'],
cs.DM.zeros(2 * xnum, nSteps))
# classify the resulting constraints
tmp_order_a = get_order(cs.vertcat(*model.ode), [
syms[i] for i in range(pnum + xnum + ynum + anum + unum + vnum)
]) # global order
tmp_order_c = get_order(
cs.vertcat(*model.ode),
[syms[i] for i in range(pnum + xnum + ynum + anum + unum)
]) # order considering the continuous variables only
labels_tmp += 2 * [
'<ocp_int' + str(i) + '>' + '<semi_cvx>' *
int(tmp_order_a[i] > 1) * int(tmp_order_c[i] <= 1) +
'<non-convex>' * int(tmp_order_c[i] > 1) for i in range(xnum)
]
# constraints for discrete transitions
if model.itr != []:
# define discrete transition
itr = cs.Function('itr', [
cs.vertcat(tsym, psyms, xsyms, ysyms, asyms, isyms, usyms, vsyms,
dtsym)
], [cs.vertcat(*model.itr)])
# evaluate the discrete transitions
itr = itr.map(nSteps)
trans = itr(
cs.vertcat(shooting['t'][:-1], shooting['x'][:, :-1],
shooting['y'][:, :-1], shooting['a'][:, :-1],
shooting['p'][:, :-1], shooting['i'][:, :-1],
shooting['u'], shooting['v'], shooting['dt']))
# add discrete transition constraints
if not opts['leq0_only']:
problem.cns['exp'] = cs.vertcat(
problem.cns['exp'],
shooting['y'][:, 1:] - shooting['y'][:, :-1] - trans)
problem.cns['lob'] = cs.vertcat(problem.cns['lob'],
cs.DM.zeros(trans.size()))
problem.cns['upb'] = cs.vertcat(problem.cns['upb'],
cs.DM.zeros(trans.size()))
# classify the resulting constraints
tmp_order_a = get_order(cs.vertcat(*model.itr), [
syms[i] for i in range(pnum + xnum + ynum + anum + unum + vnum)
]) # global order
tmp_order_c = get_order(
cs.vertcat(*model.itr),
[syms[i] for i in range(pnum + xnum + ynum + anum + unum)
]) # order considering the continuous variables only
labels_tmp += [
'<ocp_dst' + str(i) + '>' + '<semi_cvx>' *
int(tmp_order_a[i] > 1) * int(tmp_order_c[i] <= 1) +
'<non-convex>' * int(tmp_order_c[i] > 1) for i in range(ynum)
]
else:
problem.cns['exp'] = cs.vertcat(
problem.cns['exp'],
shooting['y'][:, 1:] - shooting['y'][:, :-1] - trans,
-shooting['y'][:, 1:] + shooting['y'][:, :-1] + trans)
problem.cns['lob'] = cs.vertcat(
problem.cns['lob'],
-cs.DM.inf(trans.shape[0] * 2, trans.shape[1]))
problem.cns['upb'] = cs.vertcat(
problem.cns['upb'],
cs.DM.zeros(trans.shape[0] * 2, trans.shape[1]))
# classify the resulting constraints
tmp_order_a = get_order(cs.vertcat(*model.itr), [
syms[i] for i in range(pnum + xnum + ynum + anum + unum + vnum)
]) # global order
tmp_order_c = get_order(
cs.vertcat(*model.itr),
[syms[i] for i in range(pnum + xnum + ynum + anum + unum)
]) # order considering the continuous variables only
labels_tmp += 2 * [
'<ocp_dst' + str(i) + '>' + '<semi_cvx>' *
int(tmp_order_a[i] > 1) * int(tmp_order_c[i] <= 1) +
'<non-convex>' * int(tmp_order_c[i] > 1) for i in range(ynum)
]
# reshape the constraints vector and add labels
problem.cns['exp'] = cs.reshape(problem.cns['exp'], (-1, 1))
problem.cns['lob'] = cs.reshape(problem.cns['lob'], (-1, 1))
problem.cns['upb'] = cs.reshape(problem.cns['upb'], (-1, 1))
problem.cns['lbl'] = labels_tmp * nSteps
# add initial constraints
if model.icns != [] and opts['with_initial_cnss']:
if not opts['leq0_only']:
problem.cns['exp'] = cs.vertcat(cs.substitute(cs.vertcat(*[c.exp for c in model.icns]),\
cs.vertcat(psyms,xsyms,ysyms,asyms,isyms),\
cs.vertcat(shooting['p'][:,0],shooting['x'][:,0],shooting['y'][:,0],shooting['a'][:,0],shooting['i'][:,0])),\
problem.cns['exp'])
problem.cns['lob'] = cs.vertcat(
cs.vertcat(*[c.lob for c in model.icns]), problem.cns['lob'])
problem.cns['upb'] = cs.vertcat(
cs.vertcat(*[c.upb for c in model.icns]), problem.cns['upb'])
problem.cns['lbl'] = [
model.icns[i].lbl + '<ocp_ic' + str(i) + '>'
for i in range(len(model.icns))
] + problem.cns['lbl']
else:
icnsexp = cs.vertcat(*([c.lob - c.exp for c in model.icns if c.lob > -cs.DM.inf()]+\
[c.exp - c.upb for c in model.icns if c.upb < cs.DM.inf()]))
problem.cns['exp'] = cs.vertcat(cs.substitute(icnsexp,\
cs.vertcat(psyms,xsyms,ysyms,asyms,isyms),\
cs.vertcat(shooting['p'][:,0],shooting['x'][:,0],shooting['y'][:,0],shooting['a'][:,0],shooting['i'][:,0])),\
problem.cns['exp'])
problem.cns['lob'] = cs.vertcat(-cs.DM.inf(icnsexp.shape[0], 1),
problem.cns['lob'])
problem.cns['upb'] = cs.vertcat(cs.DM.zeros(icnsexp.shape[0], 1),
problem.cns['upb'])
problem.cns['lbl'] = [c.lbl + '<ocp_ic'+str(i)+'>' for i,c in enumerate(model.icns) if c.lob > -cs.DM.inf()] +\
[c.lbl + '<ocp_ic'+str(i)+'>' for i,c in enumerate(model.icns) if c.upb < cs.DM.inf()] +\
problem.cns['lbl']
# add final constraints
if model.fcns != [] and opts['with_final_cnss']:
if not opts['leq0_only']:
problem.cns['exp'] = cs.vertcat(problem.cns['exp'],\
cs.substitute(cs.vertcat(*[c.exp for c in model.fcns]),\
cs.vertcat(psyms,xsyms,ysyms,asyms,isyms),\
cs.vertcat(shooting['p'][:,-1],shooting['x'][:,-1],shooting['y'][:,-1],shooting['a'][:,-1],shooting['i'][:,-1])))
problem.cns['lob'] = cs.vertcat(
problem.cns['lob'], cs.vertcat(*[c.lob for c in model.fcns]))
problem.cns['upb'] = cs.vertcat(
problem.cns['upb'], cs.vertcat(*[c.upb for c in model.fcns]))
problem.cns['lbl'] = problem.cns['lbl'] + [
model.fcns[i].lbl + '<ocp_fc' + str(i) + '>'
for i in range(len(model.fcns))
]
else:
fcnsexp = cs.vertcat(*([c.lob - c.exp for c in model.fcns if c.lob > -cs.DM.inf()]+\
[c.exp - c.upb for c in model.fcns if c.upb < cs.DM.inf()]))
problem.cns['exp'] = cs.vertcat(problem.cns['exp'],\
cs.substitute(fcnsexp,\
cs.vertcat(psyms,xsyms,ysyms,asyms,isyms),\
cs.vertcat(shooting['p'][:,-1],shooting['x'][:,-1],shooting['y'][:,-1],shooting['a'][:,-1],shooting['i'][:,-1])))
problem.cns['lob'] = cs.vertcat(problem.cns['lob'],
-cs.DM.inf(fcnsexp.shape[0], 1))
problem.cns['upb'] = cs.vertcat(problem.cns['upb'],
cs.DM.zeros(fcnsexp.shape[0], 1))
problem.cns['lbl'] = problem.cns['lbl'] +\
[c.lbl + '<ocp_ic'+str(i)+'>' for i,c in enumerate(model.fcns) if c.lob > -cs.DM.inf()] +\
[c.lbl + '<ocp_ic'+str(i)+'>' for i,c in enumerate(model.fcns) if c.upb < cs.DM.inf()]
## Construct the objective
if unpack_objective:
problem.obj = {'exp': cs.MX.zeros(shooting['t'].numel()), 'val': None}
else:
problem.obj = {'exp': cs.MX(0.0), 'val': None}
# add lagrangian objective
if not model.lag is None:
# define integrator
l0 = cs.MX.sym('l0')
intg = integrate_ode(
l0,
cs.vertcat(tsym, psyms, xsyms, ysyms, asyms, isyms, usyms, vsyms),
[model.lag], dtsym, opts['integration_opts']).map(nSteps)
# evaluate integration
tmp = intg.call({'x0':cs.DM.zeros(nSteps).T,\
'p':cs.vertcat(shooting['t'][:-1],shooting['x'][:,:-1],shooting['y'][:,:-1],shooting['a'][:,:-1],shooting['p'][:,:-1],shooting['i'][:,:-1],shooting['u'],shooting['v'],shooting['dt'])})['xf']
if unpack_objective:
problem.obj['exp'][:-1] += tmp.T
else:
problem.obj['exp'] += cs.sum2(tmp)
# add discrete penalties
if not model.ipn is None:
ipnf = cs.Function('ipn', [
cs.vertcat(tsym, psyms, xsyms, ysyms, asyms, isyms, usyms, vsyms,
dtsym)
], [model.ipn]).map(nSteps)
tmp = ipnf(
cs.vertcat(shooting['t'][:-1], shooting['x'][:, :-1],
shooting['y'][:, :-1], shooting['a'][:, :-1],
shooting['p'][:, :-1], shooting['i'][:, :-1],
shooting['u'], shooting['v'], shooting['dt']))
if unpack_objective:
problem.obj['exp'][:-1] += tmp.T
else:
problem.obj['exp'] += cs.sum2(tmp)
# add mayer objective
if not model.may is None:
tmp = cs.vertcat(problem.obj['exp'],cs.substitute(model.may,\
cs.vertcat(tsym,psyms,xsyms,ysyms,asyms,isyms),\
cs.vertcat(shooting['t'][:,-1],shooting['p'][:,-1],shooting['x'][:,-1],shooting['y'][:,-1],shooting['a'][:,-1],shooting['i'][:,-1])))
tmp = cs.substitute(model.may,\
cs.vertcat(tsym,psyms,xsyms,ysyms,asyms,isyms),\
cs.vertcat(shooting['t'][:,-1],shooting['p'][:,-1],shooting['x'][:,-1],shooting['y'][:,-1],shooting['a'][:,-1],shooting['i'][:,-1]))
if unpack_objective:
problem.obj['exp'][-1] += tmp
else:
problem.obj['exp'] += tmp
# add sos1 constraints
if not model.sos1 is None:
problem.sos1 = []
varsname = [v['nme'] for v in vardata]
for c in range(len(model.sos1)):
indxs = [
i for i in range(len(varsname))
if varsname[i] in model.sos1[c].group
]
if model.sos1[c].weights != []:
problem.sos1.append({'g': [[t*numVars + i for i in indxs] for t in range(nSteps)],\
'w': [cs.substitute(model.sos1[c].weights,cs.vertcat(tsym,psyms,xsyms,ysyms,asyms,isyms),cs.vertcat(shooting['t'][:,-1],shooting['p'][:,-1],shooting['x'][:,-1],shooting['y'][:,-1],shooting['a'][:,-1],shooting['i'][:,-1])) for t in range(nSteps)]})
else:
problem.sos1.append({
'g':
[[t * numVars + i for i in indxs] for t in range(nSteps)],
'w': [None] * nSteps
})
return
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 simplify(self, options):
if options.get('replace_parameter_expressions', False):
logger.info("Replacing parameter expressions")
simple_parameters, symbols, values = [], [], []
for p in self.parameters:
value = ca.MX(p.value)
if value.is_constant():
simple_parameters.append(p)
else:
symbols.append(p.symbol)
values.append(value)
self.parameters = simple_parameters
if len(values) > 0:
# Resolve expressions that include other, non-simple parameter
# expressions.
converged = False
while not converged:
new_values = ca.substitute(values, symbols, values)
converged = ca.is_equal(ca.veccat(*values),
ca.veccat(*new_values),
CASADI_COMPARISON_DEPTH)
values = new_values
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols,
values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(
self.initial_equations, symbols, values)
# Replace parameter expressions in metadata
for variable in itertools.chain(self.states, self.alg_states,
self.inputs, self.parameters,
self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value,
ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('replace_constant_expressions', False):
logger.info("Replacing constant expressions")
simple_constants, symbols, values = [], [], []
for c in self.constants:
value = ca.MX(c.value)
if value.is_constant():
simple_constants.append(c)
else:
symbols.append(c.symbol)
values.append(value)
self.constants = simple_constants
if len(values) > 0:
# Resolve expressions that include other, non-simple parameter
# expressions.
converged = False
while not converged:
new_values = ca.substitute(values, symbols, values)
converged = ca.is_equal(ca.veccat(*values),
ca.veccat(*new_values),
CASADI_COMPARISON_DEPTH)
values = new_values
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols,
values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(
self.initial_equations, symbols, values)
# Replace constant expressions in metadata
for variable in itertools.chain(self.states, self.alg_states,
self.inputs, self.parameters,
self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value,
ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('eliminate_constant_assignments', False):
logger.info("Elimating constant variable assignments")
alg_states = OrderedDict([(s.symbol.name(), s)
for s in self.alg_states])
reduced_equations = []
for eq in self.equations:
if eq.is_symbolic() and eq.name() in alg_states:
constant = alg_states.pop(eq.name())
constant.value = 0.0
self.constants.append(constant)
# Skip this equation
continue
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB)
or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(0).name(
) in alg_states and eq.dep(1).is_constant():
variable = eq.dep(0)
value = eq.dep(1)
elif eq.dep(1).is_symbolic() and eq.dep(1).name(
) in alg_states and eq.dep(0).is_constant():
variable = eq.dep(1)
value = eq.dep(0)
else:
variable = None
value = None
if variable is not None:
constant = alg_states.pop(variable.name())
if eq.is_op(ca.OP_SUB):
constant.value = value
else:
constant.value = -value
self.constants.append(constant)
# Skip this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
self.alg_states = list(alg_states.values())
self.equations = reduced_equations
if options.get('replace_parameter_values', False):
logger.info("Replacing parameter values")
# N.B. Any parameter expression elimination must be done first.
unspecified_parameters, symbols, values = [], [], []
for p in self.parameters:
if ca.MX(p.value).is_constant() and ca.MX(
p.value).is_regular():
symbols.append(p.symbol)
values.append(p.value)
else:
unspecified_parameters.append(p)
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations,
symbols, values)
self.parameters = unspecified_parameters
# Replace parameter values in metadata
for variable in itertools.chain(self.states, self.alg_states,
self.inputs, self.parameters,
self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('replace_constant_values', False):
logger.info("Replacing constant values")
# N.B. Any parameter expression elimination must be done first.
symbols = self._symbols(self.constants)
values = [v.value for v in self.constants]
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations,
symbols, values)
self.constants = []
# Replace constant values in metadata
for variable in itertools.chain(self.states, self.alg_states,
self.inputs, self.parameters,
self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('eliminable_variable_expression', None) is not None:
logger.info(
"Elimating variables that match the regular expression {}".
format(options['eliminable_variable_expression']))
p = re.compile(options['eliminable_variable_expression'])
alg_states = OrderedDict([(s.symbol.name(), s)
for s in self.alg_states])
variables = []
values = []
reduced_equations = []
for eq in self.equations:
if eq.is_symbolic() and eq.name() in alg_states and p.match(
eq.name()):
variables.append(eq)
values.append(0.0)
del alg_states[eq.name()]
# Skip this equation
continue
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB)
or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(0).name(
) in alg_states and p.match(eq.dep(0).name()):
variable = eq.dep(0)
value = eq.dep(1)
elif eq.dep(1).is_symbolic() and eq.dep(1).name(
) in alg_states and p.match(eq.dep(1).name()):
variable = eq.dep(1)
value = eq.dep(0)
else:
variable = None
value = None
if variable is not None:
del alg_states[variable.name()]
variables.append(variable)
if eq.is_op(ca.OP_SUB):
values.append(value)
else:
values.append(-value)
# Skip this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
self.alg_states = list(alg_states.values())
self.equations = reduced_equations
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, variables,
values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations,
variables, values)
if options.get('expand_vectors', False):
logger.info("Expanding vectors")
symbols = []
values = []
for l in [
'states', 'der_states', 'alg_states', 'inputs',
'parameters', 'constants'
]:
old_vars = getattr(self, l)
new_vars = []
for old_var in old_vars:
if old_var.symbol.numel() > 1:
rows = []
for i in range(old_var.symbol.size1()):
cols = []
for j in range(old_var.symbol.size2()):
if old_var.symbol.size1(
) > 1 and old_var.symbol.size2() > 1:
component_symbol = ca.MX.sym(
'{}[{},{}]'.format(
old_var.symbol.name(), i, j))
elif old_var.symbol.size1() > 1:
component_symbol = ca.MX.sym(
'{}[{}]'.format(
old_var.symbol.name(), i))
elif old_var.symbol.size2() > 1:
component_symbol = ca.MX.sym(
'{}[{}]'.format(
old_var.symbol.name(), j))
else:
raise AssertionError
component_var = Variable(
component_symbol, old_var.python_type)
for attribute in CASADI_ATTRIBUTES:
value = ca.MX(getattr(old_var, attribute))
if value.size1() == old_var.symbol.size1(
) and value.size2(
) == old_var.symbol.size2():
setattr(component_var, attribute,
value[i, j])
elif value.size1() == old_var.symbol.size1(
):
setattr(component_var, attribute,
value[i])
elif value.size2() == old_var.symbol.size2(
):
setattr(component_var, attribute,
value[j])
else:
assert value.size1() == 1
assert value.size2() == 1
setattr(component_var, attribute,
value)
cols.append(component_var)
rows.append(cols)
symbols.append(old_var.symbol)
values.append(
ca.vertcat(*[
ca.horzcat(*[v.symbol for v in row])
for row in rows
]))
new_vars.extend(itertools.chain.from_iterable(rows))
else:
new_vars.append(old_var)
setattr(self, l, new_vars)
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, symbols, values)
self.equations = list(
itertools.chain.from_iterable(
ca.vertsplit(ca.vec(eq)) for eq in self.equations))
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations,
symbols, values)
self.initial_equations = list(
itertools.chain.from_iterable(
ca.vertsplit(ca.vec(eq))
for eq in self.initial_equations))
# Replace values in metadata
for variable in itertools.chain(self.states, self.alg_states,
self.inputs, self.parameters,
self.constants):
for attribute in CASADI_ATTRIBUTES:
value = getattr(variable, attribute)
if isinstance(value, ca.MX) and not value.is_constant():
[value] = ca.substitute([value], symbols, values)
setattr(variable, attribute, value)
if options.get('factor_and_simplify_equations', False):
# Operations that preserve the equivalence of an equation
# TODO: There may be more, but this is the most frequent set
unary_ops = ca.OP_NEG, ca.OP_FABS, ca.OP_SQRT
binary_ops = ca.OP_MUL, ca.OP_DIV
# Recursive factor and simplify function
def factor_and_simplify(eq):
# These are ops that can simply be dropped
if eq.n_dep() == 1 and eq.op() in unary_ops:
return factor_and_simplify(eq.dep())
# These are binary ops and can get a little tricky
# For now, we just drop constant divisors or multipliers
elif eq.n_dep() == 2 and eq.op() in binary_ops:
if eq.dep(1).is_constant():
return factor_and_simplify(eq.dep(0))
elif eq.dep(0).is_constant() and eq.op() == ca.OP_MUL:
return factor_and_simplify(eq.dep(1))
# If no hits, return unmodified
return eq
# Do the simplifications
simplified_equations = [
factor_and_simplify(eq) for eq in self.equations
]
# Debugging output
if logger.getEffectiveLevel() == logging.DEBUG:
changed_equations = [
(o, s)
for o, s in zip(self.equations, simplified_equations)
if o is not s
]
for orig, simp in changed_equations:
logger.debug('Equation {} simplified to {}'.format(
orig, simp))
# Store changes
self.equations = simplified_equations
if options.get('detect_aliases', False):
logger.info("Detecting aliases")
states = OrderedDict([(s.symbol.name(), s) for s in self.states])
der_states = OrderedDict([(s.symbol.name(), s)
for s in self.der_states])
alg_states = OrderedDict([(s.symbol.name(), s)
for s in self.alg_states])
inputs = OrderedDict([(s.symbol.name(), s) for s in self.inputs])
parameters = OrderedDict([(s.symbol.name(), s)
for s in self.parameters])
all_states = OrderedDict()
all_states.update(states)
all_states.update(der_states)
all_states.update(alg_states)
all_states.update(inputs)
all_states.update(parameters)
alias_rel = AliasRelation()
# For now, we only eliminate algebraic states.
do_not_eliminate = set(
list(der_states) + list(states) + list(inputs) +
list(parameters))
reduced_equations = []
for eq in self.equations:
if eq.n_dep() == 2 and (eq.is_op(ca.OP_SUB)
or eq.is_op(ca.OP_ADD)):
if eq.dep(0).is_symbolic() and eq.dep(1).is_symbolic():
if eq.dep(0).name() in alg_states:
alg_state = eq.dep(0)
other_state = eq.dep(1)
elif eq.dep(1).name() in alg_states:
alg_state = eq.dep(1)
other_state = eq.dep(0)
else:
alg_state = None
other_state = None
# If both states are algebraic, we need to decide which to eliminate
if eq.dep(0).name() in alg_states and eq.dep(
1).name() in alg_states:
# Most of the time it does not matter which one we eliminate.
# The exception is if alg_state has already been aliased to a
# variable in do_not_eliminate. If this is the case, setting the
# states in the default order will cause the new canonical variable
# to be other_state, unseating (and eliminating) the current
# canonical variable (which is in do_not_eliminate).
if alias_rel.canonical_signed(
alg_state.name())[0] in do_not_eliminate:
# swap the states
other_state, alg_state = alg_state, other_state
if alg_state is not None:
# Check to see if we are linking two entries in do_not_eliminate
if alias_rel.canonical_signed(alg_state.name())[0] in do_not_eliminate and \
alias_rel.canonical_signed(other_state.name())[0] in do_not_eliminate:
# Don't do anything for now, we only eliminate alg_states
pass
else:
# Eliminate alg_state by aliasing it to other_state
if eq.is_op(ca.OP_SUB):
alias_rel.add(other_state.name(),
alg_state.name())
else:
alias_rel.add(other_state.name(),
'-' + alg_state.name())
# To keep equations balanced, drop this equation
continue
# Keep this equation
reduced_equations.append(eq)
# Eliminate alias variables
variables, values = [], []
for canonical, aliases in alias_rel:
canonical_state = all_states[canonical]
python_type = canonical_state.python_type
start = canonical_state.start
m, M = canonical_state.min, canonical_state.max
nominal = canonical_state.nominal
fixed = canonical_state.fixed
for alias in aliases:
if alias[0] == '-':
sign = -1
alias = alias[1:]
else:
sign = 1
alias_state = all_states[alias]
variables.append(alias_state.symbol)
values.append(sign * canonical_state.symbol)
# If any of the aliases has a nonstandard type, apply it to
# the canonical state as well
if alias_state.python_type != float:
python_type = alias_state.python_type
# If any of the aliases has a nondefault start value, apply it to
# the canonical state as well
alias_state_start = ca.MX(alias_state.start)
if alias_state_start.is_regular(
) and not alias_state_start.is_zero():
start = sign * alias_state.start
# The intersection of all bound ranges applies
m = ca.fmax(
m, alias_state.min if sign == 1 else -alias_state.max)
M = ca.fmin(
M, alias_state.max if sign == 1 else -alias_state.min)
# Take the largest nominal of all aliases
nominal = ca.fmax(nominal, alias_state.nominal)
# If any of the aliases is fixed, the canonical state is as well
fixed = ca.fmax(fixed, alias_state.fixed)
del all_states[alias]
canonical_state.aliases = aliases
canonical_state.python_type = python_type
canonical_state.start = start
canonical_state.min = m
canonical_state.max = M
canonical_state.nominal = nominal
canonical_state.fixed = fixed
self.states = [v for k, v in all_states.items() if k in states]
self.der_states = [
v for k, v in all_states.items() if k in der_states
]
self.alg_states = [
v for k, v in all_states.items() if k in alg_states
]
self.inputs = [v for k, v in all_states.items() if k in inputs]
self.parameters = [
v for k, v in all_states.items() if k in parameters
]
self.equations = reduced_equations
if len(self.equations) > 0:
self.equations = ca.substitute(self.equations, variables,
values)
if len(self.initial_equations) > 0:
self.initial_equations = ca.substitute(self.initial_equations,
variables, values)
if options.get('reduce_affine_expression', False):
logger.info("Collapsing model into an affine expression")
for equation_list in ['equations', 'initial_equations']:
equations = getattr(self, equation_list)
if len(equations) > 0:
states = ca.veccat(*self._symbols(
itertools.chain(self.states, self.der_states,
self.alg_states, self.inputs)))
constants = ca.veccat(*self._symbols(self.constants))
parameters = ca.veccat(*self._symbols(self.parameters))
equations = ca.veccat(*equations)
Af = ca.Function('Af', [states, constants, parameters],
[ca.jacobian(equations, states)])
bf = ca.Function('bf', [states, constants, parameters],
[equations])
# Work around CasADi issue #172
if len(self.constants) == 0 or not ca.depends_on(
equations, constants):
constants = 0
else:
logger.warning(
'Not all constants have been eliminated. As a result, the affine DAE expression will use a symbolic matrix, as opposed to a numerical sparse matrix.'
)
if len(self.parameters) == 0 or not ca.depends_on(
equations, parameters):
parameters = 0
else:
logger.warning(
'Not all parameters have been eliminated. As a result, the affine DAE expression will use a symbolic matrix, as opposed to a numerical sparse matrix.'
)
A = Af(0, constants, parameters)
b = bf(0, constants, parameters)
# Replace veccat'ed states with brand new state vectors so as to avoid the value copy operations induced by veccat.
self._states_vector = ca.MX.sym(
'states_vector',
sum([s.numel() for s in self._symbols(self.states)]))
self._der_states_vector = ca.MX.sym(
'der_states_vector',
sum([
s.numel() for s in self._symbols(self.der_states)
]))
self._alg_states_vector = ca.MX.sym(
'alg_states_vector',
sum([
s.numel() for s in self._symbols(self.alg_states)
]))
self._inputs_vector = ca.MX.sym(
'inputs_vector',
sum([s.numel() for s in self._symbols(self.inputs)]))
states_vector = ca.vertcat(self._states_vector,
self._der_states_vector,
self._alg_states_vector,
self._inputs_vector)
equations = [
ca.reshape(ca.mtimes(A, states_vector),
equations.shape) + b
]
setattr(self, equation_list, equations)
if options.get('expand_mx', False):
logger.info("Expanded MX functions will be returned")
self._expand_mx_func = lambda x: x.expand()
logger.info("Finished model simplification")
def convert_expr_from_tau_to_time(self, expr, t_k, t_kp1):
t = self.t
tau = self.tau
h = t_kp1 - t_k
return substitute(expr, tau, (t - t_k) / h)
def construct_upd_xz(self, problem=None):
# construct optifather & give reference to problem
self.father_updx = OptiFather(self.group.values())
self.problem.father = self.father_updx
# define z_ij variables
init = self.q_ij_struct(0)
for nghb, q_ij in self.q_ij.items():
for child, q_j in q_ij.items():
for name, ind in q_j.items():
var = np.array(child._values[name])
v = var.T.flatten()[ind]
init[nghb.label, child.label, name, ind] = v
z_ij = self.define_variable(
'z_ij', self.q_ij_struct.shape[0], value=np.array(init.cat))
# define parameters
l_ij = self.define_parameter('l_ij', self.q_ij_struct.shape[0])
l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0])
# put them in the struct format
z_ij = self.q_ij_struct(z_ij)
l_ij = self.q_ij_struct(l_ij)
l_ji = self.q_ji_struct(l_ji)
# get (part of) variables
x_i = self._get_x_variables(symbolic=True)
# construct local copies of parameters
par = {}
for name, s in self.par_global.items():
par[name] = self.define_parameter(name, s.shape[0], s.shape[1])
if problem is None:
# get time info
t = self.define_symbol('t')
T = self.define_symbol('T')
t0 = t/T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline(x_i, tf, self.q_i)
self._transform_spline([z_ij, l_ij], tf, self.q_ij)
self._transform_spline(l_ji, tf, self.q_ji)
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label][name]
for nghb in self.q_ji.keys():
l = l_ji[str(nghb), child.label, name]
obj += mtimes(l.T, x)
for nghb, q_j in self.q_ij.items():
for child in q_j.keys():
for name in q_j[child].keys():
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj -= mtimes(l.T, z)
self.define_objective(obj)
# construct constraints
for con in self.global_constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = x_i[label][name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_global.items():
if s.name() == sym.name():
c = substitute(c, sym, par[name])
lb, ub = con[1], con[2]
self.define_constraint(c, lb, ub)
# construct problem
prob, buildtime = self.father_updx.construct_problem(
self.options, str(self._index), problem)
self.problem_upd_xz = prob
self.father_updx.init_transformations(self.problem.init_primal_transform,
self.problem.init_dual_transform)
self.init_var_dd()
return buildtime
def plot(self, p=[['states'], ['constraints']], tikz=False, show=True):
"""Plot states and/or constraints.
Args:
p (list of lists): Each inner list defines a plot window. The
inner lists either contain the strings 'states' and/or
'constraints', the name of a state defined in ``self.sys`` or a
CasADi expression in terms of the flat outputs. When giving a
CasADi expression you can optionally supply a y-label by passing
it as a tuple.
tikz (boolean): When true the plot is saved to a .tikz file
which you can include in you latex documents to produce
beautiful plots (default False)
show (boolean): When True the plot is shown by invoking show()
from matplotlib (default True)
"""
import matplotlib.pyplot as plt
for plots in p:
fig = plt.figure()
j = 1
m = (len(plots) + ('states' in plots) * (self.sys._nx - 1) +
('constraints' in plots) * (len(self.constraints) - 1))
for l in plots:
if l in self.sys.x.keys():
ax = fig.add_subplot(m / int(np.sqrt(m)) + np.mod(m, 2),
int(np.sqrt(m)), j)
ax.plot(self.sol['t'], self.sol['states'][l])
plt.ylabel(l)
plt.xlabel('time')
j += 1
elif (l == 'states'):
for i, k in enumerate(self.sol[l].keys()):
ax = fig.add_subplot(m / int(np.sqrt(m)) + np.mod(m, 2),
int(np.sqrt(m)), j)
ax.plot(self.sol['t'], self.sol[l][k])
plt.ylabel(k)
plt.xlabel('time')
j += 1
elif l == 'constraints':
for i, f in enumerate(self.constraints):
F = cas.substitute(f[0], self.sys.y, self.path)
F = cas.SXFunction([self.s], [F])
F.init()
Flb = cas.SXFunction([self.s], [f[1]])
Fub = cas.SXFunction([self.s], [f[2]])
c = np.array([evalf(F, s.T).toArray().ravel()
for s in self.sol['s']])
cub = np.array([evalf(Fub, s.T).toArray().ravel()
for s in self.sol['s']])
clb = np.array([evalf(Flb, s.T).toArray().ravel()
for s in self.sol['s']])
ax = fig.add_subplot(m / int(np.sqrt(m)) + np.mod(m, 2),
int(np.sqrt(m)), j)
ax.plot(self.sol['t'], c)
ax.plot(self.sol['t'], clb, color='black')
ax.plot(self.sol['t'], cub, color='black')
j += 1
elif isinstance(l, tuple):
if isinstance(l[1], cas.SXMatrix):
F = cas.substitute(l[1], self.sys.y, self.path)
F = cas.SXFunction([self.s], [F])
F.init()
c = np.array([evalf(F, s.T).toArray().ravel()
for s in self.sol['s']])
ax = fig.add_subplot(m / int(np.sqrt(m)) + np.mod(m, 2),
int(np.sqrt(m)), j)
ax.plot(self.sol['t'], c)
else:
ax = fig.add_subplot(m / int(np.sqrt(m)) + np.mod(m, 2),
int(np.sqrt(m)), j)
ax.plot(self.sol['t'], l[1])
plt.ylabel(l[0])
plt.xlabel('time')
j += 1
else:
if isinstance(l, cas.SXMatrix):
F = cas.substitute(l, self.sys.y, self.path)
F = cas.SXFunction([self.s], [F])
F.init()
c = np.array([evalf(F, s.T).toArray().ravel()
for s in self.sol['s']])
ax = fig.add_subplot(m / int(np.sqrt(m)) + np.mod(m, 2),
int(np.sqrt(m)), j)
ax.plot(self.sol['t'], c)
else:
ax = fig.add_subplot(m / int(np.sqrt(m)) + np.mod(m, 2),
int(np.sqrt(m)), j)
ax.plot(self.sol['t'], l)
plt.xlabel('time')
j += 1
if tikz and len(p) == 1:
from matplotlib2tikz import save as tsave
tsave('_'.join(p[0]) + '.tikz')
if show:
plt.show()
def exit_classDefinition(self, tree: etree._Element): # noqa: too-complex
dae = HybridDae()
dae.t = self.scope['time']
self.model[tree] = dae
# handle component declarations
for var_name, v in self.scope['var'].items():
variability = self.scope['prop'][var_name]['variability']
if variability == 'continuous':
if var_name in self.scope['states']:
dae.x = ca.vertcat(dae.x, v)
dae.dx = ca.vertcat(dae.dx, self.der(v))
else:
dae.y = ca.vertcat(dae.y, v)
elif variability == 'discrete':
dae.m = ca.vertcat(dae.m, v)
elif variability == 'parameter':
dae.p = ca.vertcat(dae.p, v)
elif variability == 'constant':
dae.p = ca.vertcat(dae.p, v)
else:
raise ValueError('unknown variability', variability)
for eq in self.scope['eqs']:
if isinstance(eq, self.sym):
dae.f_x = ca.vertcat(dae.f_x, eq)
# build reinit expression and discrete equations
dae.f_i = dae.x
dae.f_m = dae.m
for eq in self.scope['when_eqs']:
w = eq['cond']
for then_eq in eq['then']:
if isinstance(then_eq, tuple):
if then_eq[0] == 'reinit':
sub_var = then_eq[1]
sub_expr = ca.if_else(self.edge(w), then_eq[2], sub_var)
dae.f_i = ca.substitute(dae.f_i, sub_var, sub_expr)
elif isinstance(then_eq, self.sym):
# this is a discrete variable assignment
# so it should be a casadi subtraction y = x
assert then_eq.is_op(ca.OP_SUB) and then_eq.n_dep() == 2
sub_var = then_eq.dep(0)
sub_expr = ca.if_else(self.edge(w), then_eq.dep(1), sub_var)
dae.f_m = ca.substitute(dae.f_m, sub_var, sub_expr)
dae.t = self.scope['time']
dae.prop.update(self.scope['prop'])
c_dict = self.scope['c']
for k in c_dict.keys():
dae.c = ca.vertcat(dae.c, k)
dae.pre_c = ca.vertcat(dae.pre_c, self.pre_cond(k))
dae.f_c = ca.vertcat(dae.f_c, c_dict[k])
for l, r in [('f_c', 'c'), ('c', 'pre_c'), ('dx', 'x'), ('f_m', 'm')]:
vl = getattr(dae, l)
vr = getattr(dae, r)
if vl.shape != vr.shape:
raise ValueError(
'{:s} and {:s} must have the same shape:'
'\n{:s}: {:s}\t{:s}: {:s}'.format(
l, r, l, str(dae.f_m), r, str(dae.m)))
dae.ng = ca.vertcat(*self.scope['ng'])
dae.nu = ca.vertcat(*self.scope['nu'])
n_eq = dae.f_x.shape[0] + dae.f_m.shape[0]
n_var = dae.x.shape[0] + dae.m.shape[0] + dae.y.shape[0]
if n_eq != n_var:
raise ValueError(
'must have equal number of equations '
'{:d} and unknowns {:d}\n:{:s}'.format(
n_eq, n_var, str(dae)))
self.scope_stack.pop()
#!/usr/bin/env python
# Benchmark my casadi-based dynamic equations
import casadi
from carousel_model import DAE_RHS, ODE_RHS, q, dq, ddq, constants, inputs
# Specialize dynamic equations for the constants (modeling parameters)
eqns = casadi.substitute(ODE_RHS,constants.keys(),constants.values())
ins = casadi.vertcat([q,dq,inputs])
f = casadi.SXFunction([ins],[eqns])
f.init()
f.generateCode('carousel_model.c')
#from cse import cse
#from carousel_model import ODE_RHS as f
#cn = casadi.countNodes
#f2 = cse(f)
#print cn(f)
#print cn(f2)
def substitute_nodes(self, f):
return [
casadi.substitute([f], sub[0], sub[1])[0]
for sub in self.node_substitutions
]
def subsForModel(self, ll):
subs = self._getSubs()
params = casadi.vertcat(*[getattr(self, p) for p, _ in subs])
paramVals = casadi.vertcat(*[val for _, val in subs])
return casadi.substitute(ll, params, paramVals)
def _make_constraints(self):
"""Parse the constraints and put them in the correct format"""
N = self.options['N']
Nc = self.options['Nc']
b = self.prob['vars'][0]
H = self.prob['vars'][1]
# ODEs
# ~~~~
con = self._ode(b)
lb = np.alen(con) * [0]
ub = np.alen(con) * [0]
# Convex combination
# ~~~~~~~~~~~~~~~~~~
con.append(cas.sumCols(H))
lb.extend([1] * Nc)
ub.extend([1] * Nc)
# Sample constraints
# ~~~~~~~~~~~~~~~~~~
S = self.prob['s']
path, bs = self._make_path()[0:2]
basisH = self._make_basis()
B = [np.matrix(basisH(S))]
# TODO!!! ============================================================
for i in range(1, self.h.size2()):
# B.append(np.matrix(basisH.derivative(S, i)))
Bi, p = basisH.derivative(i)
B.append(np.matrix(np.dot(Bi(S), p)))
# ====================================================================
for f in self.constraints:
F = cas.substitute(f[0], self.sys.y, path)
if f[3] is None:
F = cas.vertcat([cas.substitute(F,
cas.vertcat([
self.s[0],
bs,
cas.vec(self.h)]),
cas.vertcat([
S[j],
cas.vec(b[j, :]),
cas.vec(cas.vertcat([cas.mul(B[i][j, :], H) for i in range(self.h.size2())]).trans())
])) for j in range(N + 1)])
Flb = [evalf(cas.SXFunction([self.s], [cas.SXMatrix(f[1])]), s).toArray().ravel() for s in S]
Fub = [evalf(cas.SXFunction([self.s], [cas.SXMatrix(f[2])]), s).toArray().ravel() for s in S]
con.append(F)
lb.extend(Flb)
ub.extend(Fub)
else:
F = cas.vertcat([cas.substitute(F,
cas.vertcat([
self.s[0],
bs,
cas.vec(self.h)]),
cas.vertcat([
S[j],
cas.vec(b[j, :]),
cas.vec(cas.vertcat([cas.mul(B[i][j, :], H) for i in range(self.h.size2())]).trans())
])) for j in f[3]])
con.append(F)
lb.extend([f[1]])
ub.extend([f[2]])
self.prob['con'] = [con, lb, ub]
def fprime(x):
ret= casadi.substitute(jacob, params, casadi.vertcat(*x))
ret= numpy.array([float(ret[i]) for i in xrange(ret.shape[1])])
return ret
def _create_problem(self, model, sampled_parameter):
# Problem
problem = OptimalControlProblem(model)
problem.name = self.socp.name + '_PCE'
problem.p_opt = substitute(self.socp.p_opt,
self.socp.get_p_without_p_unc(),
problem.model.p_sym)
problem.theta_opt = substitute(self.socp.theta_opt,
self.socp.model.theta_sym,
problem.model.theta_sym)
problem.x_max = repmat(vertcat(self.socp.x_max, inf), self.n_samples)
problem.y_max = repmat(self.socp.y_max, self.n_samples)
problem.u_max = self.socp.u_max
problem.delta_u_max = self.socp.delta_u_max
problem.p_opt_max = self.socp.p_opt_max
problem.theta_opt_max = self.socp.theta_opt_max
problem.x_min = repmat(vertcat(self.socp.x_min, -inf), self.n_samples)
problem.y_min = repmat(self.socp.y_min, self.n_samples)
problem.u_min = self.socp.u_min
problem.delta_u_min = self.socp.delta_u_min
problem.p_opt_min = self.socp.p_opt_min
problem.theta_opt_min = self.socp.theta_opt_min
problem.t_f = self.socp.t_f
if depends_on(self.socp.g_eq, self.socp.model.x_sym) or depends_on(
self.socp.g_eq, self.socp.model.y_sym):
raise NotImplementedError(
'Case where "g_eq" depends on "model.x_sym" or "model.y_sym" is not implemented '
)
if depends_on(self.socp.g_ineq, self.socp.model.x_sym) or depends_on(
self.socp.g_ineq, self.socp.model.y_sym):
raise NotImplementedError(
'Case where "g_ineq" depends on "model.x_sym" '
'or "model.y_sym" is not implemented ')
original_vars = vertcat(self.socp.model.u_sym,
self.socp.get_p_without_p_unc(),
self.socp.model.theta_sym,
self.socp.model.t_sym, self.socp.model.tau_sym)
new_vars = vertcat(problem.model.u_sym, problem.model.p_sym,
problem.model.theta_sym, problem.model.t_sym,
problem.model.tau_sym)
if not self.socp.n_h_initial == self.socp.model.n_x:
problem.h_initial = vertcat(
problem.h_initial,
substitute(self.socp.h_initial[:self.socp.model.n_x],
original_vars, new_vars))
problem.h_final = substitute(self.socp.h_final, original_vars,
new_vars)
problem.g_eq = substitute(self.socp.g_eq, original_vars, new_vars)
problem.g_ineq = substitute(self.socp.g_ineq, original_vars, new_vars)
problem.time_g_eq = self.socp.time_g_eq
problem.time_g_ineq = self.socp.time_g_ineq
for i in range(self.socp.n_uncertain_initial_condition):
ind = self.socp.get_uncertain_initial_cond_indices()[i]
x_ind_s = problem.model.x_0_sym[ind::(self.socp.model.n_x + 1)]
problem.h_initial = substitute(
problem.h_initial, x_ind_s,
sampled_parameter[self.socp.n_p_unc + i, :].T)
problem.h_final = substitute(
problem.h_final, x_ind_s,
sampled_parameter[self.socp.n_p_unc + i, :].T)
problem.g_eq = substitute(
problem.g_eq, x_ind_s,
sampled_parameter[self.socp.n_p_unc + i, :].T)
problem.g_ineq = substitute(
problem.g_ineq, x_ind_s,
sampled_parameter[self.socp.n_p_unc + i, :].T)
problem.last_u = self.socp.last_u
problem.y_guess = repmat(
self.socp.y_guess,
self.n_samples) if self.socp.y_guess is not None else None
problem.u_guess = self.socp.u_guess
problem.x_0 = repmat(
vertcat(self.socp.x_0,
0), self.n_samples) if self.socp.x_0 is not None else None
problem.parametrized_control = self.socp.parametrized_control
problem.positive_objective = self.socp.parametrized_control
problem.NULL_OBJ = self.socp.NULL_OBJ
if not is_equal(self.socp.S, 0) or not is_equal(self.socp.V, 0):
raise NotImplementedError
return problem
