diff --git a/nosnoc/__init__.py b/nosnoc/__init__.py index 15d1158..b68fd97 100644 --- 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 index 8523ab4..bc9ea26 100644 --- 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 index 0a4a41b..a012ea4 100644 --- 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 index 6658e1c..fdac4bd 100644 --- 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__":