Add experimental MpccMode.FISCHER_BURMEISTER_IP_AUG (#24)

* collect cost_values, use np.copy

* move initialize to base solver

* fix typehint

* add another smoothing parameter tau, MpccMode.FISCHER_BURMEISTER_IP_AUG

* fix typo

* fix MpccMode.FISCHER_BURMEISTER_IP_AUG

* use tau instead of sqrt(tau)

* oscilator example: store reference solution, add least_squares example

* add relay least_squares example

* docstrings NosnocOcp

* add check

* comment json stuff

examples/oscilator_example.py

@@ -58,11 +58,68 @@ def main():
     looper.run()
     results = looper.get_results()
 
+
     plot_oscilator(results["X_sim"], results["t_grid"])
     nosnoc.plot_timings(results["cpu_nlp"])
-    import pdb
+    # store solution
+    # import json
+    # json_file = 'oscilator_results_ref.json'
+    # with open(json_file, 'w') as f:
+    #     json.dump(results['w_sim'], f, indent=4, sort_keys=True, default=make_object_json_dumpable)
+    # print(f"saved results in {json_file}")
+
+
+def main_least_squares():
+
+    # load reference solution
+    # import json
+    # json_file = 'oscilator_results_ref.json'
+    # with open(json_file, 'r') as f:
+    #     w_sim_ref = json.load(f)
+
+    opts = nosnoc.NosnocOpts()
+    # opts.irk_representation = "differential"
+    opts.use_fesd = True
+    comp_tol = 1e-7
+    opts.comp_tol = comp_tol
+    # opts.homotopy_update_slope = 0.9  # decrease rate
+    opts.N_finite_elements = 2
+    opts.n_s = 2
+    opts.print_level = 3
+
+    # opts.homotopy_update_rule = nosnoc.HomotopyUpdateRule.SUPERLINEAR
+    opts.cross_comp_mode = nosnoc.CrossComplementarityMode.COMPLEMENT_ALL_STAGE_VALUES_WITH_EACH_OTHER
+    opts.mpcc_mode = nosnoc.MpccMode.FISCHER_BURMEISTER_IP_AUG
+    opts.constraint_handling = nosnoc.ConstraintHandling.LEAST_SQUARES
+    opts.step_equilibration = nosnoc.StepEquilibrationMode.DIRECT
+    opts.initialization_strategy = nosnoc.InitializationStrategy.ALL_XCURRENT_W0_START
+    # opts.initialization_strategy = nosnoc.InitializationStrategy.RK4_SMOOTHENED
+    opts.sigma_0 = 1e0
+    # opts.gamma_h = np.inf
+    # opts.opts_casadi_nlp['ipopt']['max_iter'] = 0
+    # opts.homotopy_update_rule = nosnoc.HomotopyUpdateRule.SUPERLINEAR
+    opts.homotopy_update_slope = 0.1
+
+    model = get_oscilator_model()
+
+    Tsim = np.pi / 2
+    Nsim = 29
+    Tstep = Tsim / Nsim
+
+    opts.terminal_time = Tstep
+
+    solver = nosnoc.NosnocSolver(opts, model)
+    solver.print_problem()
+    # loop
+    looper = nosnoc.NosnocSimLooper(solver, model.x0, Nsim)
+    # looper = nosnoc.NosnocSimLooper(solver, model.x0, Nsim, w_init=w_sim_ref)
+    looper.run()
+    results = looper.get_results()
+    print(f"max cost_val = {max(results['cost_vals']):.2e}")
 
-    pdb.set_trace()
+    plot_oscilator(results["X_sim"], results["t_grid"])
+    nosnoc.plot_timings(results["cpu_nlp"])
+    # breakpoint()
 
 
 def plot_oscilator(X_sim, t_grid, latexify=True):
@@ -98,5 +155,12 @@ def plot_oscilator(X_sim, t_grid, latexify=True):
     plt.show()
 
 
+def make_object_json_dumpable(input):
+    if isinstance(input, (np.ndarray)):
+        return input.tolist()
+    else:
+        raise TypeError(f"Cannot make input of type {type(input)} dumpable.")
+
 if __name__ == "__main__":
     main()
+    # main_least_squares()

examples/relay_feedback_system.py

@@ -108,6 +108,58 @@ def main():
     plt.show()
 
 
+
+def main_least_squares():
+    Tsim = 10
+    Nsim = 200
+    Tstep = Tsim / Nsim
+
+    opts = get_default_options()
+    opts.terminal_time = Tstep
+
+    # LSQ
+    opts.cross_comp_mode = nosnoc.CrossComplementarityMode.COMPLEMENT_ALL_STAGE_VALUES_WITH_EACH_OTHER
+    opts.mpcc_mode = nosnoc.MpccMode.FISCHER_BURMEISTER_IP_AUG
+    opts.constraint_handling = nosnoc.ConstraintHandling.LEAST_SQUARES
+    opts.step_equilibration = nosnoc.StepEquilibrationMode.DIRECT
+    opts.initialization_strategy = nosnoc.InitializationStrategy.ALL_XCURRENT_W0_START
+    opts.print_level = 1
+    opts.sigma_0 = 1e0
+    opts.comp_tol = 1e-8
+
+    model = get_relay_feedback_system_model()
+
+    solver = nosnoc.NosnocSolver(opts, model)
+    n_exec = 1
+    for i in range(n_exec):
+        # simulation loop
+        looper = nosnoc.NosnocSimLooper(solver, model.x0, Nsim)
+        looper.run()
+        results = looper.get_results()
+        if i == 0:
+            timings = results["cpu_nlp"]
+        else:
+            timings = np.minimum(timings, results["cpu_nlp"])
+
+    print(f"max cost_val = {max(results['cost_vals'])}")
+
+    # plot trajectory
+    X_sim = results["X_sim"]
+    t_grid = results["t_grid"]
+    plot_system_trajectory(X_sim, t_grid=t_grid, figure_filename='relay_traj.pdf')
+    plot_algebraic_variables(results, figure_filename='relay_algebraic_traj.pdf')
+    # plot_system_3d(results)
+
+    # plot timings
+    filename = ""
+    filename = f"relay_timings_{datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f')}.pdf"
+    plot_title = f"{opts.irk_representation.name.lower()} IRK, init {opts.initialization_strategy.name.lower()}"  # {opts.homotopy_update_rule.name}"
+    nosnoc.plot_timings(results["cpu_nlp"], title=plot_title, figure_filename=filename)
+
+    plt.show()
+
+
+
 def main_rk4_simulation():
     """
     This examples uses a smoothened STEP representation of the system and simulates it with an RK4 integrator.
@@ -206,4 +258,5 @@ def plot_algebraic_variables(results, figure_filename=''):
 
 if __name__ == "__main__":
     main()
+    # main_least_squares()
     # main_rk4_simulation()

nosnoc/helpers.py

@@ -36,6 +36,7 @@ def __init__(self, solver: NosnocSolver, x0: np.ndarray, Nsim: int, p_values: Op
         self.alpha_sim = []
         self.w_sim = []
         self.w_all = []
+        self.cost_vals = []
         self.w_init = w_init
 
         self.cpu_nlp = np.zeros((Nsim, solver.opts.max_iter_homotopy))
@@ -61,6 +62,8 @@ def run(self) -> None:
             self.alpha_sim.append(results["alpha_list"])
             self.w_sim += [results["w_sol"]]
             self.w_all += [results["w_all"]]
+            self.cost_vals.append(results["cost_val"])
+
 
     def get_results(self) -> dict:
         self.t_grid = np.concatenate((np.array([0.0]), np.cumsum(self.time_steps)))
@@ -74,5 +77,6 @@ def get_results(self) -> dict:
             "lambda_sim": self.lambda_sim,
             "w_sim": self.w_sim,
             "w_all": self.w_all,
+            "cost_vals": self.cost_vals
         }
         return results