def __init__(self, **kwargs): super().__init__(**kwargs) self.u = SX([]) self._parametrized_controls = [] self.u_par = vertcat(self.u) self.u_expr = vertcat(self.u)
def test_parametrize_control_wrong_size(model): model.t = SX.sym('t') model.create_control('u', 1) # wrong size for expr k = SX.sym("k", 2) with pytest.raises(ValueError): model.parametrize_control(model.u, k * model.t, k)
def test_parametrize_control_already_parametrize(model): model.t = SX.sym('t') model.create_control('u', 3) # test parametrize a control already parametrized model.x = SX.sym('x') k = SX.sym("k") model.parametrize_control(model.u[0], -k * model.x[0], k) with pytest.raises(ValueError): model.parametrize_control(model.u[0], k * model.t, k)
def test_parametrize_control_time_dependent_polynomial(model): model.tau = SX.sym('tau') model.create_control('u', 3) # Test parametrize by a time dependent polynomial u_par = SX.sym("u_par", 3, 2) u_expr = model.tau * u_par[:, 0] + (1 - model.tau) * u_par[:, 1] model.parametrize_control(model.u, u_expr, vec(u_par)) assert is_equal(model.u_par, vec(u_par)) assert is_equal(model.u_expr, u_expr, 30) for ind in range(model.n_u): assert is_equal(model._parametrized_controls[ind], model.u[ind])
def test_control_is_parametrized(model: ControlMixin): model.create_control('u', 4) assert not model.control_is_parametrized(model.u[0]) # error multiple controls are passed with pytest.raises(ValueError): model.control_is_parametrized(model.u) k = SX.sym('k') model.x = SX.sym('x') model.parametrize_control(model.u[0], -k * model.x[0], k) assert model.control_is_parametrized(model.u[0])
def test_include_control(model: ControlMixin): new_u_1 = SX.sym("new_u") new_u_2 = SX.sym("new_u_2", 2) model_n_u = model.n_u model.include_control(new_u_1) assert model.n_u == model_n_u + 1 assert (is_equal(model.u[-1], new_u_1)) model.include_control(new_u_2) assert model.n_u == model_n_u + 1 + 2 assert is_equal(model.u[-3], new_u_1) assert is_equal(model.u[-2:], new_u_2)
def test_include_parameter(model): new_p_1 = SX.sym("new_p") new_p_2 = SX.sym("new_p_2", 2) model_n_p = model.n_p model.include_parameter(new_p_1) assert model.n_p == model_n_p + 1 assert is_equal(model.p[-1], new_p_1) model.include_parameter(new_p_2) assert model.n_p == model_n_p + 1 + 2 assert is_equal(model.p[-3], new_p_1) assert is_equal(model.p[-2:], new_p_2)
def test_include_theta(model): new_theta_1 = SX.sym("new_theta") new_theta_2 = SX.sym("new_theta_2", 2) model_n_theta = model.n_theta model.include_theta(new_theta_1) assert model.n_theta == model_n_theta + 1 assert is_equal(model.theta[-1], new_theta_1) model.include_theta(new_theta_2) assert model.n_theta == model_n_theta + 1 + 2 assert is_equal(model.theta[-3], new_theta_1) assert is_equal(model.theta[-2:], new_theta_2)
def include_equations(self, *args, **kwargs): if callable(getattr(super(), 'include_equations', None)): super().include_equations(*args, **kwargs) ode = kwargs.pop('ode', None) x = kwargs.pop('x', None) if ode is None and x is not None: raise ValueError("`ode` is None but `x` is not None") # if is in the list form if isinstance(ode, collections.abc.Sequence): ode = vertcat(*ode) if isinstance(x, collections.abc.Sequence): x = vertcat(*x) # if ode was passed but not x, try to guess the x if x is None and ode is not None: # Check if None are all sequential, ortherwise we don't know who it belongs first_none = list(self._ode.values()).index(None) if not all(eq is None for eq in islice(self._ode.values(), 0, first_none)): raise ValueError( "ODE should be inserted on the equation form or in the list form." "You can't mix both without explicit passing the states associated with the equation." ) x = vertcat(*list(self._ode.keys())[first_none:first_none + ode.numel()]) if len(args) > 0 and ode is None: x = SX([]) ode = SX([]) # get ode and x from equality equations for eq in args: if isinstance(eq, EqualityEquation): if isinstance(eq.lhs, Derivative): ode = vertcat(ode, eq.rhs) x = vertcat(x, eq.lhs.inner) # actually include the equations if ode is not None and ode.numel() > 0: for x_i in vertcat(x).nz: if self._ode[x_i] is not None: raise Warning( f'State "{x_i}" already had an ODE associated, overriding it!' ) ode_dict = dict(self._ode) ode_dict.update({x_i: ode[ind] for ind, x_i in enumerate(x.nz)}) self._ode = ode_dict
def test_include_algebraic(model): new_y_1 = SX.sym("new_y") new_y_2 = SX.sym("new_y_2", 2) model_n_y = model.n_y alg = new_y_1 - 3 model.include_algebraic(new_y_1, alg=alg) assert model.n_y == model_n_y + 1 assert is_equal(model.y[-1], new_y_1) assert is_equal(model.alg[-1], alg, 10) model.include_algebraic(new_y_2) assert model.n_y == model_n_y + 1 + 2 assert is_equal(model.y[-3], new_y_1) assert is_equal(model.y[-2:], new_y_2)
def test_replace_variable_u_par(model): # replace a u_par new_u_par = SX.sym("new_u", model.n_u) new_u_expr = new_u_par model.replace_variable(model.u_par, new_u_par) assert is_equal(model.u_expr, new_u_expr, 30)
def create_state(self, name="x", size=1): """ Create a new state with the name "name" and size "size". Size can be an int or a tuple (e.g. (2,2)). However, the new state will be vectorized (casadi.vec) to be included in the state vector (model.x). :param name: str :param size: int|tuple :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_x = SX.sym(name, size) new_x_0 = SX.sym(name + "_0_sym", size) self.include_state(vec(new_x), ode=None, x_0=vec(new_x_0)) return new_x
def test_parametrize_control_list_input(model): model.tau = SX.sym('tau') model.create_control('u', 3) # Test for list inputs, parametrize by a time dependent polynomial u_par = SX.sym("u_par", 3, 2) u_expr = model.tau * u_par[:, 0] + (1 - model.tau) * u_par[:, 1] model.parametrize_control( [model.u[ind] for ind in range(model.n_u)], [u_expr[ind] for ind in range(model.n_u)], [vec(u_par)[ind] for ind in range(u_par.numel())], ) assert is_equal(model.u_par, vec(u_par)) assert is_equal(model.u_expr, u_expr, 30) for ind in range(model.n_u): assert is_equal(model._parametrized_controls[ind], model.u[ind])
def test_include_state(model): new_x_1 = SX.sym("new_x") new_x_2 = SX.sym("new_x_2", 2) model_n_x = model.n_x new_x_0_1 = model.include_state(new_x_1) assert model.n_x == model_n_x + 1 assert model.x_0.numel() == model_n_x + 1 assert new_x_0_1.numel() == new_x_1.numel() assert is_equal(model.x[-1], new_x_1) model_n_x = model.n_x new_x_0_2 = model.include_state(new_x_2) assert model.n_x == model_n_x + 2 assert model.x_0.numel(), model_n_x + 2 assert new_x_0_2.numel() == new_x_2.numel() assert is_equal(model.x[-3], new_x_1) assert is_equal(model.x[-2:], new_x_2)
def test_replace_variable_alg(model: AlgebraicMixin): y = model.create_algebraic_variable('y', 3) model.include_equations(alg=[-y]) # replace y original = model.y[1] replacement = SX.sym("new_y", original.numel()) model.replace_variable(original, replacement) assert not depends_on(model.alg, original) assert depends_on(model.alg, replacement)
def test_replace_variable_state(model: StateMixin): x = model.create_state('x', 3) model.include_equations(ode=[-x], x=x) # replace x original = model.x[1] replacement = SX.sym("new_x", original.numel()) model.replace_variable(original, replacement) assert not depends_on(model.ode, original) assert depends_on(model.ode, replacement)
def seq_to_SX_matrix(seq): """ In many cases this is equivalent to cs.vertcat. """ n = len(seq) # leading element: e0 = SX(seq[0]) if e0.shape == (1, 1): # we have a sequence of scalars and create a column vector res = SX(n, 1) for i, elt in enumerate(seq): res[i, 0] = elt return res else: # we assume we have a sequence of vectors and want to concatenate them (colstack) n1, n2 = e0.shape res = SX(n1, n2 * n) for i, elt in enumerate(seq): res[:, i] = elt return res
def create_theta(self, name="theta", size=1): """ Create a new parameter name "name" and size "size" :param name: str :param size: int :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_theta = SX.sym(name, size) self.include_theta(vec(new_theta)) return new_theta
def create_algebraic_variable(self, name="y", size=1): """ Create a new algebraic variable with the name "name" and size "size". Size can be an int or a tuple (e.g. (2,2)). However, the new algebraic variable will be vectorized (casadi.vec) to be included in the algebraic vector (model.y). :param str name: :param int||tuple size: :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_y = SX.sym(name, size) self.include_algebraic(vec(new_y)) return new_y
def include_state(self, var, ode=None, x_0=None): n_x = var.numel() self.x = vertcat(self.x, var) if x_0 is None: x_0 = vertcat(*[SX.sym(var_i.name()) for var_i in var.nz]) self.x_0 = vertcat(self.x_0, x_0) # crate entry for included state for ind, x_i in enumerate(var.nz): if x_i in self._ode: raise ValueError(f'State "{x_i}" already in this model') self._ode[x_i] = None if ode is not None: self.include_equations(ode=ode, x=var) return x_0
def create_control(self, name="u", size=1): """ Create a new control variable name "name" and size "size". Size can be an int or a tuple (e.g. (2,2)). However, the new control variable will be vectorized (casadi.vec) to be included in the control vector (model.u). :param name: str :param size: int :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_u = SX.sym(name, size) self.include_control(vec(new_u)) return new_u
def include_equations(self, *args, **kwargs): if callable(getattr(super(), 'include_equations', None)): super().include_equations(*args, **kwargs) alg = kwargs.pop('alg', None) if len(args) > 0 and alg is None: alg = SX([]) # in case a list of equations `y == x + u` has been passed for eq in args: if is_equality(eq): alg = vertcat(alg, eq.dep(0) - eq.dep(1)) if isinstance(alg, collections.abc.Sequence): alg = vertcat(*alg) if alg is not None: self.alg = vertcat(self.alg, alg)
def test_n_theta(model): assert model.n_theta == 0 model.theta = SX.sym("theta", 4) assert model.n_theta == 4
def __init__(self, **kwargs): super().__init__(**kwargs) self.p = SX([]) self.theta = SX([])
class ParameterMixin: def __init__(self, **kwargs): super().__init__(**kwargs) self.p = SX([]) self.theta = SX([]) @property def n_p(self): return self.p.numel() @property def n_theta(self): return self.theta.numel() @property def p_names(self): return [self.p[i].name() for i in range(self.n_p)] @property def theta_names(self): return [self.theta[i].name() for i in range(self.n_theta)] def create_parameter(self, name="p", size=1): """ Create a new parameter name "name" and size "size" :param name: str :param size: int :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_p = SX.sym(name, size) self.include_parameter(vec(new_p)) return new_p def create_theta(self, name="theta", size=1): """ Create a new parameter name "name" and size "size" :param name: str :param size: int :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_theta = SX.sym(name, size) self.include_theta(vec(new_theta)) return new_theta def include_parameter(self, p): self.p = vertcat(self.p, p) def include_theta(self, theta): self.theta = vertcat(self.theta, theta) def remove_parameter(self, var): self.p = remove_variables_from_vector(var, self.p) def remove_theta(self, var): self.theta = remove_variables_from_vector(var, self.theta)
def SX_diag_matrix(seq): n = len(seq) res = SX.zeros(n, n) for i, elt in enumerate(seq): res[i, i] = elt return res
class ControlMixin: def __init__(self, **kwargs): super().__init__(**kwargs) self.u = SX([]) self._parametrized_controls = [] self.u_par = vertcat(self.u) self.u_expr = vertcat(self.u) @property def n_u(self): return self.u.numel() @property def n_u_par(self): return self.u_par.numel() @property def u_names(self): return [self.u[i].name() for i in range(self.n_u)] def create_control(self, name="u", size=1): """ Create a new control variable name "name" and size "size". Size can be an int or a tuple (e.g. (2,2)). However, the new control variable will be vectorized (casadi.vec) to be included in the control vector (model.u). :param name: str :param size: int :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_u = SX.sym(name, size) self.include_control(vec(new_u)) return new_u def include_control(self, var): self.u = vertcat(self.u, var) self.u_expr = vertcat(self.u_expr, var) self.u_par = vertcat(self.u_par, var) def remove_control(self, var): self.u_expr = remove_variables_from_vector_by_indices( find_variables_indices_in_vector(var, self.u), self.u_expr) self.u = remove_variables_from_vector(var, self.u) self.u_par = remove_variables_from_vector(var, self.u_par) def replace_variable(self, original, replacement): if isinstance(original, list): original = vertcat(*original) if isinstance(replacement, list): replacement = vertcat(*replacement) if not original.numel() == replacement.numel(): raise ValueError( "Original and replacement must have the same number of elements!" "original.numel()={}, replacement.numel()={}".format( original.numel(), replacement.numel())) if callable(getattr(super(), 'replace_variable', None)): super().replace_variable(original, replacement) # self.u_par = substitute(self.u_par, original, replacement) self.u_expr = substitute(self.u_expr, original, replacement) def parametrize_control(self, u, expr, u_par=None): """ Parametrize a control variables so it is a function of a set of parameters or other model variables. :param list|casadi.SX u: :param list|casadi.SX expr: :param list|casadi.SX u_par: """ # input check if isinstance(u, list): u = vertcat(*u) if isinstance(u_par, list): u_par = vertcat(*u_par) if isinstance(expr, list): expr = vertcat(*expr) if not u.numel() == expr.numel(): raise ValueError( "Passed control and parametrization expression does not have same size. " "u ({}) and expr ({})".format(u.numel(), expr.numel())) # Check and register the control parametrization. for u_i in u.nz: if self.control_is_parametrized(u_i): raise ValueError( f'The control "{u_i}" is already parametrized.') # to get have a new memory address self._parametrized_controls = self._parametrized_controls + [u_i] # Remove u from u_par if they are going to be parametrized self.u_par = remove_variables_from_vector(u, self.u_par) if u_par is not None: self.u_par = vertcat(self.u_par, u_par) # Replace u by expr into the system self.replace_variable(u, expr) def create_input(self, name="u", size=1): """ Same as the "model.create_control" function. Create a new control/input variable name "name" and size "size". Size can be an int or a tuple (e.g. (2,2)). However, the new control variable will be vectorized (casadi.vec) to be included in the control vector (model.u). :param name: str :param size: int :return: """ return self.create_control(name, size) def control_is_parametrized(self, u): """ Check if the control "u" is parametrized :param casadi.SX u: :rtype bool: """ u = vertcat(u) if not u.numel() == 1: raise ValueError( 'The parameter "u" is expected to be of size 1x1, given: {}x{}' .format(*u.shape)) if any([ is_equal(u, parametrized_u) for parametrized_u in self._parametrized_controls ]): return True return False
def ode(self): try: return vertcat( *[val for val in self._ode.values() if val is not None]) except NotImplementedError: return SX.zeros(0, 1)
class StateMixin: def __init__(self, **kwargs): super().__init__(**kwargs) self.x = SX([]) self.x_0 = SX([]) self._ode = dict() @property def n_x(self): return self.x.numel() @property def x_names(self): return [self.x[i].name() for i in range(self.n_x)] @property def ode(self): try: return vertcat( *[val for val in self._ode.values() if val is not None]) except NotImplementedError: return SX.zeros(0, 1) def create_state(self, name="x", size=1): """ Create a new state with the name "name" and size "size". Size can be an int or a tuple (e.g. (2,2)). However, the new state will be vectorized (casadi.vec) to be included in the state vector (model.x). :param name: str :param size: int|tuple :return: """ if callable(getattr(self, 'name_variable', None)): name = self.name_variable(name) new_x = SX.sym(name, size) new_x_0 = SX.sym(name + "_0_sym", size) self.include_state(vec(new_x), ode=None, x_0=vec(new_x_0)) return new_x def include_state(self, var, ode=None, x_0=None): n_x = var.numel() self.x = vertcat(self.x, var) if x_0 is None: x_0 = vertcat(*[SX.sym(var_i.name()) for var_i in var.nz]) self.x_0 = vertcat(self.x_0, x_0) # crate entry for included state for ind, x_i in enumerate(var.nz): if x_i in self._ode: raise ValueError(f'State "{x_i}" already in this model') self._ode[x_i] = None if ode is not None: self.include_equations(ode=ode, x=var) return x_0 def remove_state(self, var, eq=None): self.x = remove_variables_from_vector(var, self.x) for x_i in var.nz: del self._ode[x_i] def replace_variable(self, original, replacement): if isinstance(original, list): original = vertcat(*original) if isinstance(replacement, list): replacement = vertcat(*replacement) if not original.numel() == replacement.numel(): raise ValueError( "Original and replacement must have the same number of elements!" "original.numel()={}, replacement.numel()={}".format( original.numel(), replacement.numel())) if callable(getattr(super(), 'replace_variable', None)): super().replace_variable(original, replacement) if original.numel() > 0: for x_i, x_i_eq in self._ode.items(): self._ode[x_i] = substitute(x_i_eq, original, replacement) def include_equations(self, *args, **kwargs): if callable(getattr(super(), 'include_equations', None)): super().include_equations(*args, **kwargs) ode = kwargs.pop('ode', None) x = kwargs.pop('x', None) if ode is None and x is not None: raise ValueError("`ode` is None but `x` is not None") # if is in the list form if isinstance(ode, collections.abc.Sequence): ode = vertcat(*ode) if isinstance(x, collections.abc.Sequence): x = vertcat(*x) # if ode was passed but not x, try to guess the x if x is None and ode is not None: # Check if None are all sequential, ortherwise we don't know who it belongs first_none = list(self._ode.values()).index(None) if not all(eq is None for eq in islice(self._ode.values(), 0, first_none)): raise ValueError( "ODE should be inserted on the equation form or in the list form." "You can't mix both without explicit passing the states associated with the equation." ) x = vertcat(*list(self._ode.keys())[first_none:first_none + ode.numel()]) if len(args) > 0 and ode is None: x = SX([]) ode = SX([]) # get ode and x from equality equations for eq in args: if isinstance(eq, EqualityEquation): if isinstance(eq.lhs, Derivative): ode = vertcat(ode, eq.rhs) x = vertcat(x, eq.lhs.inner) # actually include the equations if ode is not None and ode.numel() > 0: for x_i in vertcat(x).nz: if self._ode[x_i] is not None: raise Warning( f'State "{x_i}" already had an ODE associated, overriding it!' ) ode_dict = dict(self._ode) ode_dict.update({x_i: ode[ind] for ind, x_i in enumerate(x.nz)}) self._ode = ode_dict
def __init__(self, **kwargs): super().__init__(**kwargs) self.x = SX([]) self.x_0 = SX([]) self._ode = dict()