rtc-tools 2.7.3__py3-none-any.whl

rtctools/optimization/linearized_order_goal_programming_mixin.py

@@ -0,0 +1,235 @@
+import casadi as ca
+import numpy as np
+
+from rtctools.optimization.goal_programming_mixin_base import (
+    Goal,
+    StateGoal,
+    _GoalConstraint,
+    _GoalProgrammingMixinBase,
+)
+
+
+class LinearizedOrderGoal(Goal):
+    #: Override linearization of goal order. Related global goal programming
+    #: option is ``linearize_goal_order``
+    #: (see :py:meth:`LinearizedOrderGoalProgrammingMixin.goal_programming_options`).
+    #: The default value of None defers to the global option, but the user can
+    #: explicitly override it per goal by setting this value to True or False.
+    linearize_order = None
+
+    #: Coefficients to linearize a goal's order
+    _linear_coefficients = {}
+
+    @classmethod
+    def _get_linear_coefficients(cls, order, eps=0.1, kind="balanced"):
+        assert order > 1, "Order should be strictly larger than one"
+
+        try:
+            return cls._linear_coefficients[eps][order]
+        except KeyError:
+            pass
+
+        x = ca.SX.sym("x")
+        a = ca.SX.sym("a")
+        b = ca.SX.sym("b")
+
+        # Strike a balance between "absolute error < eps" and "relative error < eps" by
+        # multiplying eps with x**(order-1)
+        if kind == "balanced":
+            f = x**order - eps * x ** (order - 1) - (a * x + b)
+        elif kind == "abs":
+            f = x**order - eps - (a * x + b)
+        else:
+            raise Exception("Unknown error approximation strategy '{}'".format(kind))
+
+        res_vals = ca.Function("res_vals", [x, ca.vertcat(a, b)], [f])
+
+        do_step = ca.rootfinder("next_state", "fast_newton", res_vals)
+
+        x = 0.0
+        a = 0.0
+        b = 0.0
+
+        xs = [0.0]
+        while x < 1.0:
+            # Initial guess larger than 1.0 to always have the next point be
+            # on the right (i.e. not left) side.
+            x = float(do_step(2.0, [a, b]))
+            a = order * x ** (order - 1)
+            b = x**order - a * x
+            xs.append(x)
+
+        # Turn underestimate into an overestimate, such that we get rid of
+        # horizontal line at origin.
+        xs[-1] = 1.0
+        xs = np.array(xs)
+        ys = xs**order
+
+        a = (ys[1:] - ys[:-1]) / (xs[1:] - xs[:-1])
+        b = ys[1:] - a * xs[1:]
+        lines = list(zip(a, b))
+
+        cls._linear_coefficients.setdefault(eps, {})[order] = lines
+
+        return lines
+
+
+class LinearizedOrderStateGoal(LinearizedOrderGoal, StateGoal):
+    """
+    Convenience class definition for linearized order state goals. Note that
+    it is possible to just inherit from :py:class:`.LinearizedOrderGoal` to get the needed
+    functionality for control of the linearization at goal level.
+    """
+
+    pass
+
+
+class LinearizedOrderGoalProgrammingMixin(_GoalProgrammingMixinBase):
+    """
+    Adds support for linearization of the goal objective functions, i.e. the
+    violation variables to a certain power. This can be used to keep a problem
+    fully linear and/or make sure that no quadratic constraints appear when using
+    the goal programming option ``keep_soft_constraints``.
+    """
+
+    def goal_programming_options(self):
+        """
+        If ``linearize_goal_order`` is set to ``True``, the goal's order will be
+        approximated linearly for any goals where order > 1. Note that this option
+        does not work with minimization goals of higher order. Instead, it is
+        suggested to transform these minimization goals into goals with a target (and
+        function range) when using this option. Note that this option can be overriden
+        on the level of a goal by using a :py:class:`LinearizedOrderGoal` (see
+        :py:attr:`LinearizedOrderGoal.linearize_order`).
+        """
+        options = super().goal_programming_options()
+        options["linearize_goal_order"] = True
+        return options
+
+    def _gp_validate_goals(self, goals, is_path_goal):
+        options = self.goal_programming_options()
+
+        for goal in goals:
+            goal_linearize = None
+            if isinstance(goal, LinearizedOrderGoal):
+                goal_linearize = goal.linearize_order
+
+            if goal_linearize or (options["linearize_goal_order"] and goal_linearize is not False):
+                if not goal.has_target_bounds and goal.order > 1:
+                    raise Exception(
+                        "Higher order minimization goals not allowed with "
+                        "`linearize_goal_order` for goal {}".format(goal)
+                    )
+
+        super()._gp_validate_goals(goals, is_path_goal)
+
+    def _gp_goal_constraints(self, goals, sym_index, options, is_path_goal):
+        options = self.goal_programming_options()
+
+        def _linearize_goal(goal):
+            goal_linearize = None
+            if isinstance(goal, LinearizedOrderGoal):
+                goal_linearize = goal.linearize_order
+
+            if goal_linearize or (options["linearize_goal_order"] and goal_linearize is not False):
+                if goal.order > 1 and not goal.critical:
+                    return True
+                else:
+                    return False
+            else:
+                return False
+
+        lo_soft_constraints = [[] for ensemble_member in range(self.ensemble_size)]
+        lo_epsilons = []
+
+        # For the linearized goals, we use all of the normal processing,
+        # except for the objective. We can override the objective function by
+        # setting a _objective_func function on the Goal object.
+        for j, goal in enumerate(goals):
+            if not _linearize_goal(goal):
+                continue
+
+            assert goal.has_target_bounds, "Cannot linearize minimization goals"
+
+            # Make a linear epsilon, and constraints relating the linear
+            # variable to the original objective function
+            path_prefix = "path_" if is_path_goal else ""
+            linear_variable = ca.MX.sym(
+                path_prefix + "lineps_{}_{}".format(sym_index, j), goal.size
+            )
+
+            lo_epsilons.append(linear_variable)
+
+            if isinstance(goal, LinearizedOrderGoal):
+                coeffs = goal._get_linear_coefficients(goal.order)
+            else:
+                coeffs = LinearizedOrderGoal._get_linear_coefficients(goal.order)
+
+            epsilon_name = path_prefix + "eps_{}_{}".format(sym_index, j)
+
+            for a, b in coeffs:
+                # We add to soft constraints, as these constraints are no longer valid when
+                # having `keep_soft_constraints` = False. This is because the `epsilon` and
+                # the `linear_variable` no longer exist in the next priority.
+                for ensemble_member in range(self.ensemble_size):
+
+                    def _f(
+                        problem,
+                        goal=goal,
+                        epsilon_name=epsilon_name,
+                        linear_variable=linear_variable,
+                        a=a,
+                        b=b,
+                        ensemble_member=ensemble_member,
+                        is_path_constraint=is_path_goal,
+                    ):
+                        if is_path_constraint:
+                            eps = problem.variable(epsilon_name)
+                            lin = problem.variable(linear_variable.name())
+                        else:
+                            eps = problem.extra_variable(epsilon_name, ensemble_member)
+                            lin = problem.extra_variable(linear_variable.name(), ensemble_member)
+
+                        return lin - a * eps - b
+
+                    lo_soft_constraints[ensemble_member].append(
+                        _GoalConstraint(goal, _f, 0.0, np.inf, False)
+                    )
+
+            if is_path_goal and options["scale_by_problem_size"]:
+                goal_m, goal_M = self._gp_min_max_arrays(goal, target_shape=len(self.times()))
+                goal_active = np.isfinite(goal_m) | np.isfinite(goal_M)
+                n_active = np.sum(goal_active.astype(int), axis=0)
+            else:
+                n_active = 1
+
+            def _objective_func(
+                problem,
+                ensemble_member,
+                goal=goal,
+                linear_variable=linear_variable,
+                is_path_goal=is_path_goal,
+                n_active=n_active,
+            ):
+                if is_path_goal:
+                    lin = problem.variable(linear_variable.name())
+                else:
+                    lin = problem.extra_variable(linear_variable.name(), ensemble_member)
+
+                return goal.weight * lin / n_active
+
+            goal._objective_func = _objective_func
+
+        (
+            epsilons,
+            objectives,
+            soft_constraints,
+            hard_constraints,
+            extra_constants,
+        ) = super()._gp_goal_constraints(goals, sym_index, options, is_path_goal)
+
+        epsilons = epsilons + lo_epsilons
+        for ensemble_member in range(self.ensemble_size):
+            soft_constraints[ensemble_member].extend(lo_soft_constraints[ensemble_member])
+
+        return epsilons, objectives, soft_constraints, hard_constraints, extra_constants

rtctools/optimization/min_abs_goal_programming_mixin.py

@@ -0,0 +1,385 @@
+import functools
+import itertools
+from typing import List
+
+import casadi as ca
+import numpy as np
+
+from .goal_programming_mixin import GoalProgrammingMixin
+from .goal_programming_mixin_base import (
+    Goal,
+    StateGoal,
+    _EmptyEnsembleList,
+    _EmptyEnsembleOrderedDict,
+    _GoalConstraint,
+    _GoalProgrammingMixinBase,
+)
+from .single_pass_goal_programming_mixin import SinglePassGoalProgrammingMixin
+from .timeseries import Timeseries
+
+
+class MinAbsGoal(Goal):
+    """
+    Absolute minimization goal class which can be used to minimize the
+    absolute value of the goal's (linear) goal function. Contrary to its super
+    class, the default order is 1 as absolute minimization is typically
+    desired for fully linear problems.
+    """
+
+    order = 1
+
+
+class MinAbsStateGoal(StateGoal, MinAbsGoal):
+    pass
+
+
+class _ConvertedMinAbsGoal(Goal):
+    order = 1
+
+    def __init__(self, abs_variable, is_path_goal, orig_goal):
+        self.abs_variable = abs_variable
+        self.is_path_goal = is_path_goal
+        self.orig_goal = orig_goal
+
+        # Copy relevant properties
+        self.size = orig_goal.size
+        self.weight = orig_goal.weight
+        self.relaxation = orig_goal.relaxation / orig_goal.function_nominal
+        self.priority = orig_goal.priority
+
+    def function(self, optimization_problem, ensemble_member):
+        if self.is_path_goal:
+            return optimization_problem.variable(self.abs_variable.name())
+        else:
+            return optimization_problem.extra_variable(self.abs_variable.name(), ensemble_member)
+
+
+class MinAbsGoalProgrammingMixin(_GoalProgrammingMixinBase):
+    """
+    Similar behavior to :py:class:`.GoalProgrammingMixin`, but any
+    :py:class:`MinAbsGoal` passed to :py:meth:`.min_abs_goals` or
+    :py:meth:`.min_abs_path_goals` will be automatically converted to:
+
+      1. An auxiliary minimization variable
+      2. Two additional linear constraints relating the auxiliary variable to the goal function
+      3. A new goal (of a different type) minimizing the auxiliary variable
+    """
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+
+        # List for any absolute minimization goals
+        self.__problem_constraints = _EmptyEnsembleList()
+        self.__problem_vars = []
+        self.__problem_path_constraints = _EmptyEnsembleList()
+        self.__problem_path_vars = []
+        self.__seeds = _EmptyEnsembleOrderedDict()
+        self.__path_seeds = _EmptyEnsembleOrderedDict()
+
+        self.__first_run = True
+
+    @property
+    def extra_variables(self):
+        return super().extra_variables + self.__problem_vars
+
+    @property
+    def path_variables(self):
+        return super().path_variables + self.__problem_path_vars
+
+    def bounds(self):
+        bounds = super().bounds()
+        for abs_var in self.__problem_vars + self.__problem_path_vars:
+            bounds[abs_var.name()] = (0.0, np.inf)
+        return bounds
+
+    def seed(self, ensemble_member):
+        seed = super().seed(ensemble_member)
+
+        # Seed minimization variables of current priority (not those of
+        # previous priorities, as those are handled by GoalProgrammingMixin).
+        for abs_var, val in self.__seeds[ensemble_member].items():
+            seed[abs_var] = val
+
+        times = self.times()
+        for abs_var, val in self.__path_seeds[ensemble_member].items():
+            seed[abs_var] = Timeseries(times, val)
+
+        return seed
+
+    def constraints(self, ensemble_member):
+        constraints = super().constraints(ensemble_member)
+
+        for constraint in self.__problem_constraints[ensemble_member]:
+            constraints.append((constraint.function(self), constraint.min, constraint.max))
+
+        return constraints
+
+    def path_constraints(self, ensemble_member):
+        path_constraints = super().path_constraints(ensemble_member)
+
+        for constraint in self.__problem_path_constraints[ensemble_member]:
+            path_constraints.append((constraint.function(self), constraint.min, constraint.max))
+
+        return path_constraints
+
+    def __validate_goals(self, goals, is_path_goal):
+        goals = sorted(goals, key=lambda x: x.priority)
+
+        for goal in goals:
+            if not isinstance(goal, MinAbsGoal):
+                raise Exception(
+                    "Absolute goal not an instance of MinAbsGoal for goal {}".format(goal)
+                )
+
+            if goal.function_range != (np.nan, np.nan):
+                raise Exception(
+                    "Absolute goal function is only allowed for minimization for goal {}".format(
+                        goal
+                    )
+                )
+
+            if goal.order != 1:
+                raise Exception(
+                    "Absolute goal function is only allowed for order = 1 for goal {}".format(goal)
+                )
+
+            if goal.weight <= 0:
+                raise Exception(
+                    "Absolute goal function is only allowed for weight > 0 for goal {}".format(goal)
+                )
+
+    @staticmethod
+    def __convert_goals(goals, sym_index, ensemble_size, is_path_goal):
+        # Replace absolute minimization goals with a new goal, and some
+        # additional hard constraints.
+        constraints = [[] for ensemble_member in range(ensemble_size)]
+        variables = []
+
+        # It is easier to modify goals in place, but we do not want to modify
+        # the original input list of goals. Make a copy to work with and
+        # return when we are done.
+        goals = goals.copy()
+
+        for j, goal in enumerate(goals):
+            assert isinstance(goal, MinAbsGoal)
+
+            abs_variable_name = "abs_{}_{}".format(sym_index, j)
+            if is_path_goal:
+                abs_variable_name = "path_" + abs_variable_name
+
+            abs_variable = ca.MX.sym(abs_variable_name, goal.size)
+            variables.append(abs_variable)
+
+            # Set constraints on how the additional variable relates to the
+            # original goal function, such that it corresponds to its absolute
+            # value when minimizing.
+            for ensemble_member in range(ensemble_size):
+
+                def _constraint_func(
+                    problem,
+                    sign,
+                    abs_variable=abs_variable,
+                    ensemble_member=ensemble_member,
+                    goal=goal,
+                    is_path_goal=is_path_goal,
+                ):
+                    if is_path_goal:
+                        abs_variable = problem.variable(abs_variable.name())
+                    else:
+                        abs_variable = problem.extra_variable(abs_variable.name(), ensemble_member)
+
+                    return (
+                        abs_variable
+                        + sign * goal.function(problem, ensemble_member) / goal.function_nominal
+                    )
+
+                _pos = functools.partial(_constraint_func, sign=1)
+                _neg = functools.partial(_constraint_func, sign=-1)
+
+                constraints[ensemble_member].append(_GoalConstraint(None, _pos, 0.0, np.inf, False))
+                constraints[ensemble_member].append(_GoalConstraint(None, _neg, 0.0, np.inf, False))
+
+            # Overwrite the original goal, such that it is just a minimization
+            # of the additional variable.
+            goals[j] = _ConvertedMinAbsGoal(abs_variable, is_path_goal, goal)
+
+        return goals, constraints, variables
+
+    def __calculate_seed(self, goals, is_path_goal):
+        assert self.__first_run is False
+
+        seed = [{} for ensemble_member in range(self.ensemble_size)]
+
+        for goal in goals:
+            assert isinstance(goal, _ConvertedMinAbsGoal)
+
+            for ensemble_member in range(self.ensemble_size):
+                if is_path_goal:
+                    expr = self.map_path_expression(
+                        goal.orig_goal.function(self, ensemble_member), ensemble_member
+                    )
+                else:
+                    expr = goal.orig_goal.function(self, ensemble_member)
+
+                function = ca.Function("f", [self.solver_input], [expr])
+                value = np.array(function(self.solver_output))
+
+                assert value.ndim == 2
+
+                if goal.size == 1:
+                    if is_path_goal:
+                        value = value.ravel()
+                    else:
+                        value = value.item()
+
+                seed[ensemble_member][goal.abs_variable.name()] = np.abs(value)
+
+        return seed
+
+    def optimize(self, preprocessing=True, **kwargs):
+        # Do pre-processing
+        if preprocessing:
+            self.pre()
+
+        goals = self.min_abs_goals()
+        path_goals = self.min_abs_path_goals()
+
+        # Validate goal definitions
+        self.__validate_goals(goals, is_path_goal=False)
+        self.__validate_goals(path_goals, is_path_goal=True)
+
+        # List for absolute minimization goals. These will be incrementally
+        # filled only just before we need them to.
+        self.__problem_constraints = [[] for ensemble_member in range(self.ensemble_size)]
+        self.__problem_vars = []
+        self.__problem_path_constraints = [[] for ensemble_member in range(self.ensemble_size)]
+        self.__problem_path_vars = []
+
+        # Similar to the above, but these keep track of all auxiliary
+        # variables and constraints of priorities we have had (and therefore
+        # activated), and those yet to come (and have not yet activated).
+        self.__subproblem_constraints = {}
+        self.__subproblem_vars = {}
+        self.__subproblem_abs_goals = {}
+        self.__subproblem_path_constraints = {}
+        self.__subproblem_path_vars = {}
+        self.__subproblem_path_abs_goals = {}
+
+        # We want to have consistent naming with GPMixin for our auxiliary
+        # variables. We therefore need to loop over all priorities, regardless
+        # of whether there are any MinAbsGoals in it or not.
+        priorities = {
+            int(goal.priority)
+            for goal in itertools.chain(goals, path_goals, self.goals(), self.path_goals())
+            if not goal.is_empty
+        }
+
+        subproblems = []
+        for priority in sorted(priorities):
+            subproblems.append(
+                (
+                    priority,
+                    [
+                        goal
+                        for goal in goals
+                        if int(goal.priority) == priority and not goal.is_empty
+                    ],
+                    [
+                        goal
+                        for goal in path_goals
+                        if int(goal.priority) == priority and not goal.is_empty
+                    ],
+                )
+            )
+
+        # Rewrite absolute minimization goals.
+        self.__converted_goals = []
+        self.__converted_path_goals = []
+
+        for i, (priority, goals, path_goals) in enumerate(subproblems):
+            (
+                goals,
+                self.__subproblem_constraints[priority],
+                self.__subproblem_vars[priority],
+            ) = self.__convert_goals(goals, i, self.ensemble_size, False)
+
+            self.__converted_goals.extend(goals)
+            self.__subproblem_abs_goals[priority] = goals
+
+            (
+                path_goals,
+                self.__subproblem_path_constraints[priority],
+                self.__subproblem_path_vars[priority],
+            ) = self.__convert_goals(path_goals, i, self.ensemble_size, True)
+
+            self.__converted_path_goals.extend(path_goals)
+            self.__subproblem_path_abs_goals[priority] = path_goals
+
+        return super().optimize(**kwargs, preprocessing=False)
+
+    def priority_started(self, priority):
+        super().priority_started(priority)
+
+        # Enable constraints and auxiliary variables that we need starting
+        # from this priority when using GoalProgrammingMixin. When using
+        # SinglePassGoalProgrammingMixin, we need to add all constraints from
+        # the start.
+        if isinstance(self, GoalProgrammingMixin):
+            priorities = [priority]
+        elif isinstance(self, SinglePassGoalProgrammingMixin):
+            if self.__first_run:
+                priorities = self.__subproblem_constraints.keys()
+            else:
+                priorities = []
+
+        for p in priorities:
+            for a, b in zip(self.__problem_constraints, self.__subproblem_constraints[p]):
+                a.extend(b)
+
+            self.__problem_vars.extend(self.__subproblem_vars[p])
+
+            for a, b in zip(self.__problem_path_constraints, self.__subproblem_path_constraints[p]):
+                a.extend(b)
+
+            self.__problem_path_vars.extend(self.__subproblem_path_vars[p])
+
+        # Calculate the seed needed for goals/variables introduced in this
+        # priority. We can only calculate a seed if this is not the first
+        # priority.
+        if not self.__first_run and isinstance(self, GoalProgrammingMixin):
+            self.__seeds = self.__calculate_seed(self.__subproblem_abs_goals[priority], False)
+            self.__path_seeds = self.__calculate_seed(
+                self.__subproblem_path_abs_goals[priority], True
+            )
+
+        self.__first_run = False
+
+    def min_abs_goals(self) -> List[MinAbsGoal]:
+        """
+        User problem returns list of :py:class:`MinAbsGoal` objects.
+
+        :returns: A list of goals.
+        """
+        return []
+
+    def goals(self):
+        goals = super().goals()
+        try:
+            return goals + self.__converted_goals
+        except AttributeError:
+            return goals
+
+    def min_abs_path_goals(self) -> List[MinAbsGoal]:
+        """
+        User problem returns list of :py:class:`MinAbsGoal` objects.
+
+        :returns: A list of goals.
+        """
+        return []
+
+    def path_goals(self):
+        goals = super().path_goals()
+        try:
+            return goals + self.__converted_path_goals
+        except AttributeError:
+            return goals