def include_constraint(self, expr): """Includes an inequality or inequality to the optimization problem, Example: opt_problem.include_constraint(1 <= x**2) opt_problem.include_constraint(x + y == 1) Due to limitations on CasADi it does not allows for double inequalities (e.g.: 0 <= x <= 1) :param casadi.MX expr: equality or inequality expression """ # Check for inconsistencies if not is_inequality(expr) and not is_equality(expr): raise ValueError( "The passed 'expr' was not recognized as an equality or inequality constraint" ) if expr.dep(0).is_constant() and expr.dep(1).is_constant(): raise ValueError('Both sides of the constraint are constant') if depends_on(expr.dep(0), vertcat(self.x, self.p)) and depends_on( expr.dep(1), vertcat(self.x, self.p)): raise ValueError( "One of the sides of the constraint cannot depend on the problem. (e.g.: x<=1)" "lhs: {}, rhs: {}".format(expr.dep(0), expr.dep(1))) # find the dependent and independent term if depends_on(expr.dep(0), vertcat(self.x, self.p)): dep_term = expr.dep(0) indep_term = expr.dep(1) if indep_term.is_constant(): indep_term = indep_term.to_DM() else: dep_term = expr.dep(1) indep_term = expr.dep(0) if indep_term.is_constant(): indep_term = indep_term.to_DM() # if it is and equality if is_equality(expr): self.include_equality(dep_term, indep_term) # if it is an inequality elif is_inequality(expr): # by default all inequalities are treated as 'less than' or 'less or equal', e.g.: x<=1 or 1<=x # if the term on the rhs term is the non-symbolic term, then it is a 'x<=1' inequality, # where the independent term is the upper bound if is_equal(indep_term, expr.dep(1)): self.include_inequality(dep_term, ub=indep_term) # otherwise, it is a '1<=x' inequality, where the independent term is the lower bound else: self.include_inequality(dep_term, lb=indep_term)
def der(self, expr): if depends_on(expr, self.u): raise Exception( "Dependency on controls not supported yet for stage.der") ode = self._ode() return jtimes( expr, self.x, ode(x=self.x, u=self.u, z=self.z, p=self.p, t=self.t)["ode"])
def depends_on(self, var): """ Return True if the system of equations ('ode' and 'alg')depends on 'var' (contains 'var' in the equations). :param casadi.SX var: :rtype: bool """ return depends_on(vertcat(self.ode, self.alg), var)
def _getUsedVar(self, f): used_var = dict() for name_var, value_var in self.state_var_container.getVarAbstrDict( ).items(): if cs.depends_on(f, value_var): used_var[name_var] = value_var return used_var
def split_dae_alg(eqs: SYM, dx: SYM) -> Dict[str, SYM]: """Split equations into differential algebraic and algebraic only""" dae = [] alg = [] for eq in ca.vertsplit(eqs): if ca.depends_on(eq, dx): dae.append(eq) else: alg.append(eq) return {'dae': ca.vertcat(*dae), 'alg': ca.vertcat(*alg)}
def sqrt_covariance_predict(W, F, Q): """ Finds a sqrt factorization of the continuous time covariance propagation equations. Requires solving a linear system of equations to keep the sqrt lower triangular. 'A Square Root Formulation of the Kalman Covariance Equations', Andrews 68 W: sqrt P, symbolic, with sparsity lower triangulr F: dynamics matrix Q: process noise matrix returns: W_dot_sol: sqrt of P deriative, lower triangular """ n_x = F.shape[0] XL = ca.SX.sym('X', ca.Sparsity_lower(n_x)) X = (XL - XL.T) for i in range(n_x): X[i, i] = 0 W_dot = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X, ca.inv(W).T) # solve for XI that keeps W dot lower triangular y = ca.vertcat(*ca.triu(W_dot, False).nonzeros()) x_dep = [] for i, xi in enumerate(XL.nonzeros()): if ca.depends_on(y, xi): x_dep += [xi] x_dep = ca.vertcat(*x_dep) A = ca.jacobian(y, x_dep) for i, xi in enumerate(XL.nonzeros()): assert not ca.depends_on(A, xi) b = -ca.substitute(y, x_dep, 0) x_sol = ca.solve(A, b) X_sol = ca.SX(X) for i in range(x_dep.shape[0]): X_sol = ca.substitute(X_sol, x_dep[i], x_sol[i]) X_sol = ca.sparsify(X_sol) W_dot_sol = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X_sol, ca.inv(W).T) return W_dot_sol
def include_variable(self, variable, lb=-inf, ub=inf): """Include a symbolic variable in the optimization problem :param variable: variable to be included :param lb: Lower bound of the variable. If the given variable size is greater than one but a scalar is passed as lower bound, a vector of lb with size of the given variable will be used as a lower bound. (default = [-inf]*size) :param ub: Upper bound of the variable. If the given variable size is greater than one but a scalar is passed as upper bound, a vector of ub with size of the given variable will be used as a upper bound. (default = [inf]*size) """ lb = vertcat(lb) ub = vertcat(ub) if lb.numel() == 1 and variable.numel() > 1: lb = repmat(lb, variable.numel()) if ub.numel() == 1 and variable.numel() > 1: ub = repmat(ub, variable.numel()) if not variable.numel() == lb.numel() or not variable.numel( ) == ub.numel(): raise ValueError( "Lower bound or upper bound has different size of the given variable" ) if not lb.is_constant() and depends_on( lb, self.p) or not ub.is_constant() and depends_on(ub, self.p): raise ValueError( "Neither the lower or the upper bound can depend on the optimization problem parameter. " "lb={}, ub={}".format(lb, ub)) for i in range(variable.numel()): if lb[i] > ub[i]: raise ValueError( 'Lower bound is greater than upper bound for index {}. ' 'The inequality {} <= {} <= is infeasible'.format( i, lb[i], variable[i], ub[i])) self.x = vertcat(self.x, variable) self.x_lb = vertcat(self.x_lb, lb) self.x_ub = vertcat(self.x_ub, ub)
def split_dae_alg(eqs: SYM, dx: SYM) -> Dict[str, SYM]: """Split equations into differential algebraic and algebraic only""" dae = [] alg = [] for eq in ca.vertsplit(eqs): if ca.depends_on(eq, dx): dae.append(eq) else: alg.append(eq) return { 'dae': ca.vertcat(*dae), 'alg': ca.vertcat(*alg) }
def is_signal(self, expr): """Does the expression represent a signal (does it depend on time)? Returns ------- res : bool """ return depends_on( expr, vertcat(self.x, self.u, self.t, vcat(self.variables['control'] + self.variables['states'])))
def setCostFunction(self, name, j, nodes=None): # TODO check if variable exists in nodes (BUG: if added from node 0 the variable x-1, it does NOT give error) used_var = dict() # select from all variables only the variables used by the added constrain function for name_var, var in list(self.var_opt.items()): if cs.depends_on(j, var): used_var[name_var] = var f = cs.Function(name, list(used_var.values()), [j]) if not nodes: nodes = [0, self.N] if any(isinstance(el, list) for el in nodes): n_chunks = len(nodes) else: n_chunks = 1 cost_function = dict(cost_function=f, var=list(used_var.keys()), nodes=nodes) ## determine if there are lists in list of nodes if n_chunks > 1: if len(nodes) > len(set([item for sublist in nodes for item in sublist])): raise Exception('Intersecting lists of nodes.') ## if nodes of constraints is outside the range of nodes in the problem, trim if n_chunks > 1: if max(nodes)[0] > self.N: warnings.warn('WARNING: lists of constraints nodes (max: {0}) outside the problem nodes (max: {1}). Removing.'.format([max(nodes)[0], max(nodes)[1]], self.N)) nodes.remove(max(nodes)) if max(nodes)[1] > self.N: warnings.warn('WARNING: lists of constraints nodes (max: {0}) outside the problem nodes (max: {1}). Trimming.'.format(max(nodes)[1], self.N)) max(nodes)[1] = self.N else: if nodes[0] > self.N: raise Exception('WARNING: lists of constraints nodes (max: {0}) outside the problem nodes (max: {1}).'.format(nodes[0], self.N)) if nodes[1] > self.N: warnings.warn('WARNING: lists of constraints(max: {0}) nodes outside the problem nodes (max: {1}). Trimming.'.format(nodes[1], self.N)) nodes[1] = self.N self.j_dict[name] = cost_function
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 include_equality(self, expr, rhs=None): """Include a equality with the following form expr = rhs :param expr: expression, this is the only term that should contain symbolic variables :param rhs: right hand side, by default it is a vector of zeros with same size of expr. If the 'expr' size is greater than one but a scalar is passed as 'rhs', a vector of 'rhs' with size of 'expr' will be used as right hand side. (default = [0]*size) """ if isinstance(expr, list): expr = vertcat(expr) if expr.size2() > 1: raise Exception( "Given expression is not a vector, number of columns is {}". format(expr.size2())) if rhs is None: rhs = DM.zeros(expr.shape) else: rhs = vertcat(rhs) if rhs.numel() == 1 and expr.numel() > 1: rhs = repmat(rhs, expr.numel()) if not expr.shape == rhs.shape: msg = "Expression and the right hand side does not have the same size: " \ "expr.shape={}, rhs.shape=={}".format(expr.shape, rhs.shape) raise ValueError(msg) # check for if rhs have 'x's and 'p's if depends_on(rhs, vertcat(self.x, self.p)): raise ValueError( "Right-hand side cannot contain variables from the optimization problem. " "RHS = {}".format(rhs)) self.g = vertcat(self.g, expr) self.g_lb = vertcat(self.g_lb, rhs) self.g_ub = vertcat(self.g_ub, rhs)
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 ast.Symbol.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 ast.Symbol.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 ast.Symbol.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 ast.Symbol.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 ast.Symbol.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 ast.Symbol.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('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]) all_states = OrderedDict() all_states.update(states) all_states.update(der_states) all_states.update(alg_states) all_states.update(inputs) alias_rel = AliasRelation() # For now, we only eliminate algebraic states. do_not_eliminate = set(list(der_states) + list(states) + list(inputs)) 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.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("Expanding MX graph") if len(self.equations) > 0: self.equations = ca.matrix_expand(self.equations) if len(self.initial_equations) > 0: self.initial_equations = ca.matrix_expand(self.initial_equations) logger.info("Finished model simplification")
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) 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['codegen']: 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'] # 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, model.variable_metadata_function(ca.veccat(*[np.nan for v in model.parameters])))) for k, key in enumerate(variables_with_metadata): for i, d in enumerate(db[key]): variable = variable_dict[d['name']] for j, tmp in enumerate(CASADI_ATTRIBUTES): if ca.depends_on(ca.MX(metadata[key][i, j]), parameter_vector): setattr(variable, tmp, metadata[key][i, j]) else: setattr(variable, tmp, independent_metadata[key][i, j]) # Done return model
def save_model(model_folder: str, model_name: str, model: Model, compiler_options: Dict[str, str]) -> None: """ Saves a CasADi model to disk. :param model_folder: Folder where the precompiled CasADi model will be stored. :param model_name: Name of the model. :param model: Model instance. :param compiler_options: Dictionary of compiler options. """ objects = { 'dae_residual': None, 'initial_residual': None, 'variable_metadata': None, 'delay_arguments': None } for o in objects.keys(): f = getattr(model, o + '_function') if compiler_options.get('codegen', False): objects[o] = _codegen_model(model_folder, f, '{}_{}'.format(model_name, o)) else: objects[o] = f # Output metadata db_file = os.path.join(model_folder, model_name + ".pymoca_cache") with open(db_file, 'wb') as f: db = {} # Store version db['version'] = __version__ # Include references to the shared libraries (codegen) or pickled functions (cache) db.update(objects) db['library_os'] = os.name db['options'] = compiler_options # Describe variables per category for key in [ 'states', 'der_states', 'alg_states', 'inputs', 'parameters', 'constants' ]: db[key] = [e.to_dict() for e in getattr(model, key)] # Caching using CasADi functions will lead to constants seemingly # depending on MX variables. Figuring out that they do not is slow, # especially when doing it on a lazy function call, as would be the # case when reading from cache. So instead, we do the depency check # once when saving the model. # Metadata dependency checking parameter_vector = ca.veccat(*[v.symbol for v in model.parameters]) for k, key in enumerate( ['states', 'alg_states', 'inputs', 'parameters', 'constants']): metadata_shape = (len(getattr(model, key)), len(CASADI_ATTRIBUTES)) m = db[key + "__metadata_dependent"] = np.zeros(metadata_shape, dtype=bool) for i, v in enumerate(getattr(model, key)): for j, tmp in enumerate(CASADI_ATTRIBUTES): attr = getattr(v, tmp) if (isinstance(attr, ca.MX) and not attr.is_constant() and ca.depends_on(attr, parameter_vector)): m[i, j] = True # Delay dependency checking if model.delay_states: 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) ] symbol_to_index = {x: i for i, x in enumerate(all_symbols)} expressions, durations = zip(*model.delay_arguments) duration_dependencies = [] for dur in durations: duration_dependencies.append([ symbol_to_index[var] for var in ca.symvar(dur) if ca.depends_on(dur, var) ]) db['__delay_duration_dependent'] = duration_dependencies db['outputs'] = model.outputs db['delay_states'] = model.delay_states db['alias_relation'] = model.alias_relation pickle.dump(db, f, protocol=-1)
def depends_on(b, a): return ca.depends_on(b, a)
def save_model(model_folder: str, model_name: str, model: Model, compiler_options: Dict[str, str]) -> None: """ Saves a CasADi model to disk. :param model_folder: Folder where the precompiled CasADi model will be stored. :param model_name: Name of the model. :param model: Model instance. :param compiler_options: Dictionary of compiler options. """ objects = {'dae_residual': None, 'initial_residual': None, 'variable_metadata': None, 'delay_arguments': None} for o in objects.keys(): f = getattr(model, o + '_function') if compiler_options.get('codegen', False): objects[o] = _codegen_model(model_folder, f, '{}_{}'.format(model_name, o)) else: objects[o] = f # Output metadata db_file = os.path.join(model_folder, model_name + ".pymoca_cache") with open(db_file, 'wb') as f: db = {} # Store version db['version'] = __version__ # Include references to the shared libraries (codegen) or pickled functions (cache) db.update(objects) db['library_os'] = os.name db['options'] = compiler_options # Describe variables per category for key in ['states', 'der_states', 'alg_states', 'inputs', 'parameters', 'constants']: db[key] = [e.to_dict() for e in getattr(model, key)] # Caching using CasADi functions will lead to constants seemingly # depending on MX variables. Figuring out that they do not is slow, # especially when doing it on a lazy function call, as would be the # case when reading from cache. So instead, we do the depency check # once when saving the model. # Metadata dependency checking parameter_vector = ca.veccat(*[v.symbol for v in model.parameters]) for k, key in enumerate(['states', 'alg_states', 'inputs', 'parameters', 'constants']): metadata_shape = (len(getattr(model, key)), len(CASADI_ATTRIBUTES)) m = db[key + "__metadata_dependent"] = np.zeros(metadata_shape, dtype=bool) for i, v in enumerate(getattr(model, key)): for j, tmp in enumerate(CASADI_ATTRIBUTES): attr = getattr(v, tmp) if (isinstance(attr, ca.MX) and not attr.is_constant() and ca.depends_on(attr, parameter_vector)): m[i, j] = True # Delay dependency checking if model.delay_states: 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)] symbol_to_index = {x: i for i, x in enumerate(all_symbols)} expressions, durations = zip(*model.delay_arguments) duration_dependencies = [] for dur in durations: duration_dependencies.append( [symbol_to_index[var] for var in ca.symvar(dur) if ca.depends_on(dur, var)]) db['__delay_duration_dependent'] = duration_dependencies db['outputs'] = model.outputs db['delay_states'] = model.delay_states db['alias_relation'] = model.alias_relation pickle.dump(db, f, protocol=-1)
def include_inequality(self, expr, lb=None, ub=None): """ Include inequality to the problem with the following form lb <= expr <= ub :param expr: expression for the inequality, this is the only term that should contain symbolic variables :param lb: Lower bound of the inequality. If the 'expr' size is greater than one but a scalar is passed as lower bound, a vector of lb with size of 'expr' will be used as a lower bound. (default = [-inf]*size) :param ub: Upper bound of the inequality. If the 'expr' size is greater than one but a scalar is passed as upper bound, a vector of ub with size of 'expr' will be used as a upper bound. (default = [inf]*size) """ # check expr if isinstance(expr, list): expr = vertcat(expr) if expr.size2() > 1: raise Exception( "Given expression is not a vector, number of columns is {}". format(expr.size2())) # check lower bound if lb is None: lb = -DM.inf(expr.size1()) else: lb = vertcat(lb) if lb.numel() == 1 and expr.numel() > 1: lb = repmat(lb, expr.numel()) # check lb correct size if not expr.shape == lb.shape: raise ValueError( "Expression and lower bound does not have the same size: " "expr.shape={}, lb.shape=={}".format(expr.shape, lb.shape)) # check upper bound if ub is None: ub = DM.inf(expr.size1()) else: ub = vertcat(ub) if ub.numel() == 1 and expr.numel() > 1: ub = repmat(ub, expr.numel()) # check ub correct size if not expr.shape == ub.shape: raise ValueError( "Expression and lower bound does not have the same size: " "expr.shape={}, lb.shape=={}".format(expr.shape, ub.shape)) # check for if lb or ub have 'x's and 'p's if depends_on(vertcat(lb, ub), vertcat(self.x, self.p)): raise ValueError( "The lower and upper bound cannot contain variables from the optimization problem." "LB: {}, UB: {}".format(lb, ub)) for i in range(expr.numel()): if lb.is_constant() and ub.is_constant(): if lb[i] > ub[i]: raise ValueError( 'Lower bound is greater than upper bound for index {}. ' 'The inequality {} <= {} <= is infeasible'.format( i, lb[i], expr[i], ub[i])) self.g = vertcat(self.g, expr) self.g_lb = vertcat(self.g_lb, lb) self.g_ub = vertcat(self.g_ub, ub)
def setConstraintFunction(self, name, g, nodes=None, bounds=None): used_var = dict() # select from all variables only the variables used by the added constrain function for name_var, var in list(self.var_opt.items()): if cs.depends_on(g, var): used_var[name_var] = var # check if variable exists in the full range of nodes if name_var.find('-') != -1: # get from 'nodes' the first constrained node if any(isinstance(el, list) for el in nodes): #todo problem if its list of list or just a list duplicated code if min(nodes)[0] - int(name_var[name_var.index('-') + len('-'):]) < 0: raise Exception('Failed to add constraint: variable', name_var, 'can only be added from node n:', int(name_var[name_var.index('-') + len('-'):])) else: if nodes[0] - int(name_var[name_var.index('-') + len('-'):]) < 0: raise Exception('Failed to add constraint: variable', name_var, 'can only be added from node n:', int(name_var[name_var.index('-') + len('-'):])) print('used var in constraint:', used_var) # create function and add it to dictionary of constraint function f = cs.Function(name, list(used_var.values()), [g]) if not nodes: nodes = [0, self.N] if any(isinstance(el, list) for el in nodes): n_chunks = len(nodes) else: n_chunks = 1 constraint = dict(constraint=f, var=list(used_var.keys()), nodes=nodes, bounds=dict()) ## determine if there are lists in list of nodes if n_chunks > 1: if len(nodes) > len(set([item for sublist in nodes for item in sublist])): raise Exception('Intersecting lists of nodes.') ## if nodes of constraints is outside the range of nodes in the problem, trim if n_chunks > 1: if max(nodes)[0] > self.N: warnings.warn('WARNING: lists of constraints nodes (max: {0}) outside the problem nodes (max: {1}). Removing.'.format([max(nodes)[0], max(nodes)[1]], self.N)) nodes.remove(max(nodes)) if max(nodes)[1] > self.N: warnings.warn('WARNING: lists of constraints nodes (max: {0}) outside the problem nodes (max: {1}). Trimming.'.format(max(nodes)[1], self.N)) max(nodes)[1] = self.N else: if nodes[0] > self.N: raise Exception('WARNING: lists of constraints nodes (max: {0}) outside the problem nodes (max: {1}).'.format(nodes[0], self.N)) if nodes[1] > self.N: warnings.warn('WARNING: lists of constraints(max: {0}) nodes outside the problem nodes (max: {1}). Trimming.'.format(nodes[1], self.N)) nodes[1] = self.N ## check if list of nodes make sense # if not bounds, set all bounds as -inf, inf if not bounds: if n_chunks > 1: for chunk in nodes: for elem in range(chunk[0], chunk[1]): constraint['bounds'][elem] = dict(lbg=[-cs.inf] * g.shape[0], ubg=[cs.inf] * g.shape[0]) else: for elem in range(nodes[0], nodes[1]): constraint['bounds'][elem] = dict(lbg=[-cs.inf] * g.shape[0], ubg=[cs.inf] * g.shape[0]) # if it's a dict, propagate everything if isinstance(bounds, dict): if 'nodes' in bounds and ('lbg' not in bounds and 'ubg' not in bounds): raise Exception('Required elements "lbg" and "ubg" are not inserted.') if 'nodes' not in bounds: if n_chunks > 1: for chunk in nodes: for elem in range(chunk[0], chunk[1]): constraint['bounds'][elem] = dict() else: for elem in range(nodes[0], nodes[1]): constraint['bounds'][elem] = dict() else: if isinstance(bounds['nodes'], int): constraint['bounds'][bounds['nodes']] = dict() elif isinstance(bounds['nodes'], list): for el in range(bounds['nodes'][0], bounds['nodes'][1]): constraint['bounds'][el] = dict() if 'lbg' not in bounds: for elem in constraint['bounds']: constraint['bounds'][elem]['lbg'] = [-cs.inf] * g.shape[0] if 'ubg' not in bounds: for elem in constraint['bounds']: constraint['bounds'][elem]['ubg'] = [cs.inf] * g.shape[0] if 'lbg' in bounds and len(bounds['lbg']) != g.shape[0]: raise Exception('Dimension of lower bounds {0} does not coincide with the constraint dimension {1}'.format(len(bounds['lbg']), g.shape[0])) if 'ubg' in bounds and len(bounds['ubg']) != g.shape[0]: raise Exception('Dimension of upper bounds {0} does not coincide with the constraint dimension (1}'.format(len(bounds['ubg']), g.shape[0])) for cnsrt_elem in constraint['bounds']: if 'lbg' in bounds: constraint['bounds'][cnsrt_elem]['lbg'] = bounds['lbg'] if 'ubg' in bounds: constraint['bounds'][cnsrt_elem]['ubg'] = bounds['ubg'] elif isinstance(bounds, list): for bound in bounds: if 'nodes' not in bound: raise Exception('Missing required element "nodes".') else: # if there are more than one chunks of constraint nodes if n_chunks > 1: set_check = set() for el in nodes: if isinstance(el, list): set_check.update(list(range(el[0],el[1]))) elif isinstance(el, int): set_check.add(el) if isinstance(bound['nodes'], list): if not set(list(range(bound['nodes'][0], bound['nodes'][1]))).issubset(set_check): raise Exception('List of bound nodes outside constraints nodes.') elif isinstance(bound['nodes'], int): if not {bound['nodes']}.issubset(set_check): raise Exception('Bounds node outside constraints nodes.') else: if isinstance(bound['nodes'], list): if not set(list(range(bound['nodes'][0], bound['nodes'][1]))).issubset([nodes]): raise Exception('List of bound nodes outside constraints nodes.') elif isinstance(bound['nodes'], int): if not {bound['nodes']}.issubset([nodes]): raise Exception('Bounds node outside constraints nodes.') if isinstance(bound['nodes'], list): for node in range(bound['nodes'][0], bound['nodes'][1]): constraint['bounds'][node] = dict(lbg=bound['lbg'], ubg=bound['ubg']) elif isinstance(bound['nodes'], int): constraint['bounds'][bound['nodes']] = dict(lbg=bound['lbg'], ubg=bound['ubg']) self.g_dict[name] = constraint
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: if isinstance(p.value, list): p.value = np.array(p.value) if not np.issubdtype(p.value.dtype, np.number): raise NotImplementedError( "Only parameters arrays with numeric values can be simplified") simple_parameters.append(p) else: 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) if len(self.delay_arguments) > 0: self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, 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) if len(self.delay_arguments) > 0: self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, 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) if len(self.delay_arguments) > 0: self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, 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 len(self.delay_arguments) > 0: self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, 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: # For delayed states we do not have any reliable shape # information available due to it being an arbitrary # expression, so we just always expand. if (old_var.symbol._modelica_shape != (1,) or old_var.symbol.name() in self.delay_states): expanded_symbols = [] for ind in np.ndindex(old_var.symbol._modelica_shape): component_symbol = ca.MX.sym('{}[{}]'.format(old_var.symbol.name(), ",".join(str(i+1) for i in ind))) component_var = Variable(component_symbol, old_var.python_type) for attribute in CASADI_ATTRIBUTES: # Can't convert 3D arrays to MX, so we convert to nparray instead value = getattr(old_var, attribute) if not isinstance(value, ca.MX) and not np.isscalar(value): value = np.array(value) else: value = ca.MX(getattr(old_var, attribute)) if np.prod(value.shape) == 1: setattr(component_var, attribute, value) else: setattr(component_var, attribute, value[ind]) expanded_symbols.append(component_var) s = old_var.symbol._mx if isinstance(old_var.symbol, _MTensor) else old_var.symbol symbols.append(s) values.append(ca.reshape(ca.vertcat(*[x.symbol for x in expanded_symbols]), *tuple(reversed(s.shape))).T) new_vars.extend(expanded_symbols) # Replace variable in delay expressions and durations if needed try: assert len(self.delay_states) == len(self.delay_arguments) i = self.delay_states.index(old_var.symbol.name()) except ValueError: pass else: delay_state = self.delay_states.pop(i) delay_argument = self.delay_arguments.pop(i) for ind in np.ndindex(old_var.symbol._modelica_shape): new_name = '{}[{}]'.format(delay_state, ",".join(str(i+1) for i in ind)) self.delay_states.append(new_name) self.delay_arguments.append( DelayArgument(delay_argument.expr[ind], delay_argument.duration)) # Replace variable in list of outputs if needed try: i = self.outputs.index(old_var.symbol.name()) except ValueError: pass else: self.outputs.pop(i) for new_s in reversed(expanded_symbols): self.outputs.insert(i, new_s.symbol.name()) 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)) if len(self.delay_arguments) > 0: self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, symbols, values) # Make sure that the naming in the main loop and the delay argument loop match input_names = [v.symbol.name() for v in self.inputs] assert set(self.delay_states).issubset(input_names) # 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 len(self.delay_arguments) > 0: self.delay_arguments = self._substitute_delay_arguments(self.delay_arguments, 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 _load_model(model_folder: str, model_name: str, compiler_options: Dict[str, str]) -> CachedModel: db_file = os.path.join(model_folder, model_name) 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() # Compile shared libraries objects = { 'dae_residual': ObjectData('dae_residual', ''), 'initial_residual': ObjectData('initial_residual', ''), 'variable_metadata': ObjectData('variable_metadata', '') } # 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 pymola') if db['library_os'] != os.name: raise InvalidCacheError('Cache generated for incompatible OS') # Include references to the shared libraries for o, d in objects.items(): f = ca.external(o, db[d.key]) 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.delayed_states = db['delayed_states'] # Evaluate variable metadata: # We do this in three passes, so that we have constant attributes available through the API, # and non-constant expressions as Function calls. # 1. Extract independent parameter values metadata = dict( zip( variables_with_metadata, model.variable_metadata_function( ca.veccat(*[np.nan for v in model.parameters])))) for key in variables_with_metadata: for i, d in enumerate(db[key]): variable = variable_dict[d['name']] for j, tmp in enumerate(ast.Symbol.ATTRIBUTES): setattr(variable, tmp, metadata[key][i, j]) # 2. Plug independent values back into metadata function, to obtain values (such as bounds, or starting values) # dependent upon independent parameter values. metadata = dict( zip( variables_with_metadata, model.variable_metadata_function( ca.veccat(*[ v.value if v.value.is_regular() else np.nan for v in model.parameters ])))) for key in variables_with_metadata: for i, d in enumerate(db[key]): variable = variable_dict[d['name']] for j, tmp in enumerate(ast.Symbol.ATTRIBUTES): setattr(variable, tmp, metadata[key][i, j]) # 3. Fill in any irregular elements with expressions to be evaluated later. # Note that an expression is neccessary only if the function value actually depends on the inputs. # Otherwise, we would be dealing with a genuine NaN value. parameter_vector = ca.veccat(*[v.symbol for v in model.parameters]) metadata = dict( zip( variables_with_metadata, model.variable_metadata_function( ca.veccat(*[ v.value if v.value.is_regular() else v.symbol for v in model.parameters ])))) for k, key in enumerate(variables_with_metadata): for i, d in enumerate(db[key]): variable = variable_dict[d['name']] for j, tmp in enumerate(ast.Symbol.ATTRIBUTES): if not getattr(variable, tmp).is_regular(): if (not isinstance(metadata[key][i, j], ca.DM) and ca.depends_on(metadata[key][i, j], parameter_vector)): setattr(variable, tmp, metadata[key][i, j]) # Done return model
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) 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['codegen']: 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'] # 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, model.variable_metadata_function(ca.veccat(*[np.nan for v in model.parameters])))) for k, key in enumerate(variables_with_metadata): for i, d in enumerate(db[key]): variable = variable_dict[d['name']] for j, tmp in enumerate(CASADI_ATTRIBUTES): if ca.depends_on(ca.MX(metadata[key][i, j]), parameter_vector): setattr(variable, tmp, metadata[key][i, j]) else: setattr(variable, tmp, independent_metadata[key][i, j]) # Evaluate delay arguments: 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) if delay_arguments_raw is not None: all_symbols = ca.veccat(*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)}) for i, expr in enumerate(delay_expressions_raw): assert ca.depends_on(ca.MX(expr), all_symbols), \ "Expression to be delayed can not be a constant" if ca.depends_on(ca.MX(delay_durations_raw[i]), all_symbols): dur = delay_durations_raw[i] false_deps = ca.vertcat(*[ var for var in ca.symvar(dur) if not ca.depends_on(dur, var)]) if false_deps.size1() > 0: [dur] = ca.substitute( [dur], [false_deps], [ca.DM(np.full(false_deps.size1(), np.nan))]) 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 exitClass(self, tree): logger.debug('exitClass {}'.format(tree.name)) if tree.type == 'function': # Already handled previously self.entered_classes.pop() return ode_states = [] alg_states = [] inputs = [] constants = [] parameters = [] symbols = sorted(tree.symbols.values(), key=lambda x: x.order) for s in symbols: if 'constant' in s.prefixes: constants.append(s) elif 'parameter' in s.prefixes: parameters.append(s) elif 'input' in s.prefixes: inputs.append(s) elif 'state' in s.prefixes: ode_states.append(s) else: alg_states.append(s) self.model.states = self._ast_symbols_to_variables(ode_states) self.model.der_states = self._ast_symbols_to_variables( ode_states, differentiate=True) self.model.alg_states = self._ast_symbols_to_variables(alg_states) self.model.constants = self._ast_symbols_to_variables(constants) self.model.parameters = self._ast_symbols_to_variables(parameters) # We extend the input list, as it is already populated with delayed states. self.model.inputs.extend(self._ast_symbols_to_variables(inputs)) # The outputs are a list of strings of state names. Specifying # multiple aliases of the same state is allowed. self.model.outputs = [ v.symbol.name() for v in itertools.chain(self.model.states, self.model.alg_states) if 'output' in v.prefixes ] def discard_empty(l): return list(filter(lambda x: not ca.MX(x).is_empty(), l)) self.model.equations = discard_empty( [self.get_mx(e) for e in tree.equations]) self.model.initial_equations = discard_empty( [self.get_mx(e) for e in tree.initial_equations]) if len(tree.statements) + len(tree.initial_statements) > 0: raise NotImplementedError( 'Statements are currently supported inside functions only') # We do not support delayMax yet, so delay durations can only depend # on constants, parameters and fixed inputs. if self.model.delay_states: delay_durations = ca.veccat(*(x.duration for x in self.model.delay_arguments)) disallowed_duration_symbols = ca.vertcat( self.model.time, ca.veccat(*self.model._symbols(self.model.states)), ca.veccat(*self.model._symbols(self.model.der_states)), ca.veccat(*self.model._symbols(self.model.alg_states)), ca.veccat(*(x.symbol for x in self.model.inputs if not x.fixed))) if ca.depends_on(delay_durations, disallowed_duration_symbols): raise ValueError( "Delay durations can only depend on parameters, constants and fixed inputs." ) self.entered_classes.pop()
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