Add Superlinear Homotopy Update (#2)

* add superlinear update rule * add update rule test to test_ocp
nosnoc · Dec 8, 2022 · 068ee39 · 068ee39
1 parent ef3d703
commit 068ee39
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 29 deletions.
diff --git a/nosnoc/__init__.py b/nosnoc/__init__.py
@@ -1,5 +1,5 @@
 from .nosnoc import NosnocSolver, NosnocModel, NosnocOcp
-from .nosnoc_settings import NosnocSettings, MpccMode, IRKSchemes, StepEquilibrationMode, CrossComplementarityMode, IrkRepresentation, PssMode, IrkRepresentation
+from .nosnoc_settings import NosnocSettings, MpccMode, IRKSchemes, StepEquilibrationMode, CrossComplementarityMode, IrkRepresentation, PssMode, IrkRepresentation, HomotopyUpdateRule
 from .helpers import NosnocSimLooper
 from .utils import casadi_length
 from .plot_utils import plot_timings, latexify_plot
diff --git a/nosnoc/nosnoc.py b/nosnoc/nosnoc.py
@@ -6,13 +6,13 @@
 import numpy as np
 from casadi import SX, vertcat, horzcat, sum1, inf, Function, diag, nlpsol, fabs, tanh, mmin, transpose, fmax, fmin
 
-from nosnoc.nosnoc_settings import NosnocSettings, MpccMode, InitializationStrategy, CrossComplementarityMode, StepEquilibrationMode, PssMode, IrkRepresentation
+from nosnoc.nosnoc_settings import NosnocSettings, MpccMode, InitializationStrategy, CrossComplementarityMode, StepEquilibrationMode, PssMode, IrkRepresentation, HomotopyUpdateRule
 from nosnoc.utils import casadi_length, print_casadi_vector, casadi_vertcat_list, casadi_sum_vectors, generate_butcher_tableu, generate_butcher_tableu_integral, flatten_layer, flatten, increment_indices, validate
 
 
 @dataclass
 class NosnocModel:
-    """
+    r"""
     \dot{x} \in f_i(x, u) if x(t) in R_i \subset \R^{n_x}
 
     with R_i = {x \in \R^{n_x} | diag(S_i,\dot) * c(x) > 0}
@@ -834,7 +834,7 @@ def solve(self):
             print('sigma \t\t compl_res \t CPU time \t iter \t status')
 
         w0 = self.w0
-        sigma_k = settings.sigma_0 / settings.homotopy_update_slope
+        sigma_k = settings.sigma_0
 
         # lambda00 initialization
         x0 = w0[self.ind_x[0]]
@@ -843,7 +843,6 @@ def solve(self):
 
         # homotopy loop
         for ii in range(settings.max_iter_homotopy):
-            sigma_k = settings.homotopy_update_slope * sigma_k
             p_val[0] = sigma_k
 
             # solve NLP
@@ -879,6 +878,15 @@ def solve(self):
             if complementarity_residual < settings.comp_tol:
                 break
 
+            if sigma_k <= settings.sigma_N:
+                break
+
+            # Update the homotopy parameter.
+            if settings.homotopy_update_rule == HomotopyUpdateRule.LINEAR:
+                sigma_k = settings.homotopy_update_slope * sigma_k
+            elif settings.homotopy_update_rule == HomotopyUpdateRule.SUPERLINEAR:
+                sigma_k = max(settings.sigma_N, min(settings.homotopy_update_slope * sigma_k, sigma_k**settings.homotopy_update_exponent))
+
         # collect results
         results = dict()
         results["x_out"] = w_opt[self.ind_x[-1][-1]]

diff --git a/nosnoc/nosnoc_settings.py b/nosnoc/nosnoc_settings.py
@@ -43,6 +43,10 @@ class IrkRepresentation(Enum):
     # NOTE: tested in test_ocp
 
 
+class HomotopyUpdateRule(Enum):
+    LINEAR = 0
+    SUPERLINEAR = 1
+
 class PssMode(Enum):
     # NOTE: tested in simple_sim_tests
     STEWART = 0
@@ -74,7 +78,7 @@ class NosnocSettings:
     max_iter_homotopy: int = 0
 
     initialization_strategy: InitializationStrategy = InitializationStrategy.ALL_XCURRENT_W0_START
-    homotopy_update_slope: float = 0.1
+
     irk_representation: IrkRepresentation = IrkRepresentation.INTEGRAL
 
     # IRK and FESD Settings
@@ -104,6 +108,10 @@ class NosnocSettings:
     # MPCC and Homotopy Settings
     comp_tol: float = 1e-8
     sigma_0: float = 1.0
+    sigma_N: float = 1e-8
+    homotopy_update_slope: float = 0.1
+    homotopy_update_exponent: float = 1.5
+    homotopy_update_rule: HomotopyUpdateRule = HomotopyUpdateRule.LINEAR
 
     # step equilibration
     step_equilibration: StepEquilibrationMode = StepEquilibrationMode.HEURISTIC_DELTA

diff --git a/test/test_ocp.py b/test/test_ocp.py
@@ -25,29 +25,31 @@ def test_loop():
             for irk_representation in nosnoc.IrkRepresentation:
                 for irk_scheme in nosnoc.IRKSchemes:
                     for pss_mode in PSS_MODES:
-                        settings = get_default_settings()
-                        settings.comp_tol = 1e-5
-                        settings.N_stages = 5
-                        settings.N_finite_elements = 2
-                        settings.equidistant_control_grid = equidistant_control_grid
-                        settings.step_equilibration = step_equilibration
-                        settings.irk_representation = irk_representation
-                        settings.irk_scheme = irk_scheme
-                        settings.pss_mode = pss_mode
-
-                        print(f"test setting: {settings}")
-                        results = solve_ocp(settings)
-
-                        x_traj = results["x_traj"]
-                        u_traj = results["u_traj"]
-                        t_grid = results["t_grid"]
-
-                        np.allclose(x_traj[0], X0, atol=1e-4)
-                        np.allclose(x_traj[-1][:2], X_TARGET, atol=1e-4)
-                        np.allclose(t_grid[-1], TERMINAL_TIME, atol=1e-6)
-                        np.allclose(t_grid[0], 0.0, atol=1e-6)
-                        np.alltrue(u_traj < UBU)
-                        np.alltrue(u_traj > LBU)
+                        for homotopy_update_rule in nosnoc.HomotopyUpdateRule:
+                            settings = get_default_settings()
+                            settings.comp_tol = 1e-5
+                            settings.N_stages = 5
+                            settings.N_finite_elements = 2
+                            settings.equidistant_control_grid = equidistant_control_grid
+                            settings.step_equilibration = step_equilibration
+                            settings.irk_representation = irk_representation
+                            settings.irk_scheme = irk_scheme
+                            settings.pss_mode = pss_mode
+                            settings.homotopy_update_rule = homotopy_update_rule
+
+                            print(f"test setting: {settings}")
+                            results = solve_ocp(settings)
+
+                            x_traj = results["x_traj"]
+                            u_traj = results["u_traj"]
+                            t_grid = results["t_grid"]
+
+                            np.allclose(x_traj[0], X0, atol=1e-4)
+                            np.allclose(x_traj[-1][:2], X_TARGET, atol=1e-4)
+                            np.allclose(t_grid[-1], TERMINAL_TIME, atol=1e-6)
+                            np.allclose(t_grid[0], 0.0, atol=1e-6)
+                            np.alltrue(u_traj < UBU)
+                            np.alltrue(u_traj > LBU)
 
 
 if __name__ == "__main__":