From c444e1e1e283599c9477d1bb037fb9f679f43900 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 12 Jan 2023 13:39:32 +0100 Subject: [PATCH] Add option to formulate as least squares problem (#15) * more verbose cross_comp * structure complementarity creation to prepare more modes * simplify cross_comp removing loop over n_sys * simplify cross_comp * simplify cross_comp * fix complementarity residual * add FISCHER-BURMEISTER (WIP) * FISCHER BURMEISTER with sigma inside * fix FB term * modify plot * add ConstraintHandling option to implement Least-squares formulation * simplify step equilibration * simplify ConstraintHandling.LEAST_SQUARES * add DIRECT StepEquilibrationMode * add check * test least_squares_problem, dont test Fischer Burmeister in existing test * cleanup create_complementarity * remove redundant check * change Lambda order back * fix create_complementarity * add warning * dont test DIRECT step_equilibration in motor test --- examples/simplest_example.py | 6 +- nosnoc/__init__.py | 2 +- nosnoc/nosnoc.py | 189 ++++++++++++++++++++++------------- nosnoc/nosnoc_opts.py | 11 +- nosnoc/nosnoc_types.py | 7 ++ test/simple_sim_tests.py | 21 +++- test/test_ocp_motor.py | 19 ++-- 7 files changed, 173 insertions(+), 82 deletions(-) diff --git a/examples/simplest_example.py b/examples/simplest_example.py index 5fa2a29..21a21e5 100644 --- a/examples/simplest_example.py +++ b/examples/simplest_example.py @@ -78,7 +78,8 @@ def solve_simplest_example(opts=None, model=None): looper = nosnoc.NosnocSimLooper(solver, X0, Nsim) looper.run() results = looper.get_results() - + # solver.print_problem() + # plot_results(results) return results @@ -87,7 +88,7 @@ def plot_results(results): plt.figure() plt.subplot(3, 1, 1) - plt.plot(results["t_grid"], results["X_sim"], label='x') + plt.plot(results["t_grid"], results["X_sim"], label='x', marker='o') plt.legend() plt.grid() @@ -107,6 +108,7 @@ def plot_results(results): for i in range(n_lam): plt.plot(results["t_grid"], [x[i] for x in thetas], label=f'theta_{i}') plt.grid() + plt.vlines(results["t_grid"], ymin=0.0, ymax=1.0, linestyles='dotted') plt.legend() plt.show() diff --git a/nosnoc/__init__.py b/nosnoc/__init__.py index b838eeb..9ce7096 100644 --- a/nosnoc/__init__.py +++ b/nosnoc/__init__.py @@ -1,6 +1,6 @@ from .nosnoc import NosnocSolver, NosnocProblem, NosnocModel, NosnocOcp, get_results_from_primal_vector from .nosnoc_opts import NosnocOpts -from .nosnoc_types import MpccMode, IrkSchemes, StepEquilibrationMode, CrossComplementarityMode, IrkRepresentation, PssMode, IrkRepresentation, HomotopyUpdateRule, InitializationStrategy +from .nosnoc_types import MpccMode, IrkSchemes, StepEquilibrationMode, CrossComplementarityMode, IrkRepresentation, PssMode, IrkRepresentation, HomotopyUpdateRule, InitializationStrategy, ConstraintHandling from .helpers import NosnocSimLooper from .utils import casadi_length, print_casadi_vector from .plot_utils import plot_timings, plot_iterates, latexify_plot diff --git a/nosnoc/nosnoc.py b/nosnoc/nosnoc.py index 404ed56..73ea0ce 100644 --- a/nosnoc/nosnoc.py +++ b/nosnoc/nosnoc.py @@ -4,10 +4,10 @@ from dataclasses import dataclass, field import numpy as np -from casadi import SX, vertcat, horzcat, sum1, inf, Function, diag, nlpsol, fabs, tanh, mmin, transpose, fmax, fmin, exp +from casadi import SX, vertcat, horzcat, sum1, inf, Function, diag, nlpsol, fabs, tanh, mmin, transpose, fmax, fmin, exp, sqrt from nosnoc.nosnoc_opts import NosnocOpts -from nosnoc.nosnoc_types import MpccMode, InitializationStrategy, CrossComplementarityMode, StepEquilibrationMode, PssMode, IrkRepresentation, HomotopyUpdateRule +from nosnoc.nosnoc_types import MpccMode, InitializationStrategy, CrossComplementarityMode, StepEquilibrationMode, PssMode, IrkRepresentation, HomotopyUpdateRule, ConstraintHandling from nosnoc.utils import casadi_length, print_casadi_vector, casadi_vertcat_list, casadi_sum_list, flatten_layer, flatten, increment_indices, create_empty_list_matrix from nosnoc.rk_utils import rk4_on_timegrid @@ -476,6 +476,11 @@ def __init__(self, lbh = (1 - opts.gamma_h) * h0 self.add_step_size_variable(h, lbh, ubh, h0) + if opts.mpcc_mode in [MpccMode.SCHOLTES_EQ, MpccMode.SCHOLTES_INEQ]: + lb_dual = 0.0 + elif opts.mpcc_mode == MpccMode.FISCHER_BURMEISTER: + lb_dual = -inf + # RK stage stuff for ii in range(opts.n_s): # state derivatives @@ -495,13 +500,13 @@ def __init__(self, for ij in range(dims.n_sys): self.add_variable( SX.sym(f'theta_{ctrl_idx}_{fe_idx}_{ii+1}_{ij+1}', dims.n_f_sys[ij]), - self.ind_theta, np.zeros(dims.n_f_sys[ij]), inf * np.ones(dims.n_f_sys[ij]), + self.ind_theta, lb_dual*np.ones(dims.n_f_sys[ij]), inf * np.ones(dims.n_f_sys[ij]), opts.init_theta * np.ones(dims.n_f_sys[ij]), ii, ij) # add lambdas for ij in range(dims.n_sys): self.add_variable( SX.sym(f'lambda_{ctrl_idx}_{fe_idx}_{ii+1}_{ij+1}', dims.n_f_sys[ij]), - self.ind_lam, np.zeros(dims.n_f_sys[ij]), inf * np.ones(dims.n_f_sys[ij]), + self.ind_lam, lb_dual*np.ones(dims.n_f_sys[ij]), inf * np.ones(dims.n_f_sys[ij]), opts.init_lambda * np.ones(dims.n_f_sys[ij]), ii, ij) # add mu for ij in range(dims.n_sys): @@ -513,20 +518,21 @@ def __init__(self, for ij in range(dims.n_sys): self.add_variable( SX.sym(f'alpha_{ctrl_idx}_{fe_idx}_{ii+1}_{ij+1}', dims.n_c_sys[ij]), - self.ind_alpha, np.zeros(dims.n_c_sys[ij]), np.ones(dims.n_c_sys[ij]), + self.ind_alpha, lb_dual*np.ones(dims.n_c_sys[ij]), np.ones(dims.n_c_sys[ij]), opts.init_theta * np.ones(dims.n_c_sys[ij]), ii, ij) # add lambda_n for ij in range(dims.n_sys): self.add_variable( SX.sym(f'lambda_n_{ctrl_idx}_{fe_idx}_{ii+1}_{ij+1}', - dims.n_c_sys[ij]), self.ind_lambda_n, np.zeros(dims.n_c_sys[ij]), + dims.n_c_sys[ij]), self.ind_lambda_n, + lb_dual*np.ones(dims.n_c_sys[ij]), inf * np.ones(dims.n_c_sys[ij]), opts.init_lambda * np.ones(dims.n_c_sys[ij]), ii, ij) # add lambda_p for ij in range(dims.n_sys): self.add_variable( SX.sym(f'lambda_p_{ctrl_idx}_{fe_idx}_{ii+1}_{ij+1}', - dims.n_c_sys[ij]), self.ind_lambda_p, np.zeros(dims.n_c_sys[ij]), + dims.n_c_sys[ij]), self.ind_lambda_p, lb_dual*np.ones(dims.n_c_sys[ij]), inf * np.ones(dims.n_c_sys[ij]), opts.init_mu * np.ones(dims.n_c_sys[ij]), ii, ij) @@ -537,7 +543,7 @@ def __init__(self, for ij in range(dims.n_sys): self.add_variable( SX.sym(f'lambda_{ctrl_idx}_{fe_idx}_end_{ij+1}', dims.n_f_sys[ij]), - self.ind_lam, np.zeros(dims.n_f_sys[ij]), inf * np.ones(dims.n_f_sys[ij]), + self.ind_lam, lb_dual * np.ones(dims.n_f_sys[ij]), inf * np.ones(dims.n_f_sys[ij]), opts.init_lambda * np.ones(dims.n_f_sys[ij]), opts.n_s, ij) # add mu for ij in range(dims.n_sys): @@ -549,14 +555,14 @@ def __init__(self, for ij in range(dims.n_sys): self.add_variable( SX.sym(f'lambda_n_{ctrl_idx}_{fe_idx}_end_{ij+1}', - dims.n_c_sys[ij]), self.ind_lambda_n, np.zeros(dims.n_c_sys[ij]), + dims.n_c_sys[ij]), self.ind_lambda_n, lb_dual*np.ones(dims.n_c_sys[ij]), inf * np.ones(dims.n_c_sys[ij]), opts.init_lambda * np.ones(dims.n_c_sys[ij]), opts.n_s, ij) # add lambda_p for ij in range(dims.n_sys): self.add_variable( SX.sym(f'lambda_p_{ctrl_idx}_{fe_idx}_end_{ij+1}', - dims.n_c_sys[ij]), self.ind_lambda_p, np.zeros(dims.n_c_sys[ij]), + dims.n_c_sys[ij]), self.ind_lambda_p, lb_dual * np.ones(dims.n_c_sys[ij]), inf * np.ones(dims.n_c_sys[ij]), opts.init_mu * np.ones(dims.n_c_sys[ij]), opts.n_s, ij) @@ -588,16 +594,23 @@ def Theta(self, stage=slice(None), sys=slice(None)) -> SX: np.ones(len(flatten(self.ind_alpha[stage][sys]))) - self.w[flatten(self.ind_alpha[stage][sys])]) + def get_Theta_list(self) -> list: + return [self.Theta(stage=ii) for ii in range(len(self.ind_theta))] + def sum_Theta(self) -> SX: - Thetas = [self.Theta(stage=ii) for ii in range(len(self.ind_theta))] - return casadi_sum_list(Thetas) + return casadi_sum_list(self.get_Theta_list()) + + def get_Lambdas_incl_last_prev_fe(self, sys=slice(None)): + Lambdas = [self.Lambda(stage=ii, sys=sys) for ii in range(len(self.ind_lam))] + Lambdas += [self.prev_fe.Lambda(stage=-1, sys=sys)] + return Lambdas def sum_Lambda(self, sys=slice(None)): """NOTE: includes the prev fes last stage lambda""" - Lambdas = [self.Lambda(stage=ii, sys=sys) for ii in range(len(self.ind_lam))] - Lambdas.append(self.prev_fe.Lambda(stage=-1, sys=sys)) + Lambdas = self.get_Lambdas_incl_last_prev_fe(sys) return casadi_sum_list(Lambdas) + def h(self) -> SX: return self.w[self.ind_h] @@ -661,70 +674,96 @@ def forward_simulation(self, ocp: NosnocOcp, Uk: SX) -> None: return - def create_complementarity_constraints(self, sigma_p: SX) -> None: + + + def create_complementarity(self, x: List[SX], y: SX, sigma: SX) -> None: + """ + adds complementarity constraints corresponding to (x_i, y) for x_i in x to the FiniteElement. + + :param x: list of SX + :param y: SX + :param sigma: smoothing parameter + """ opts = self.opts - dims = self.model.dims - if not opts.use_fesd: - g_cross_comp = casadi_vertcat_list( - [diag(self.Lambda(stage=j)) @ self.Theta(stage=j) for j in range(opts.n_s)]) - elif opts.cross_comp_mode == CrossComplementarityMode.COMPLEMENT_ALL_STAGE_VALUES_WITH_EACH_OTHER: - # complement within fe - g_cross_comp = casadi_vertcat_list([ - diag(self.Theta(stage=j, sys=r)) @ self.Lambda(stage=jj, sys=r) - for r in range(dims.n_sys) for j in range(opts.n_s) for jj in range(opts.n_s) - ]) - # complement with end of previous fe - g_cross_comp = casadi_vertcat_list([g_cross_comp] + [ - diag(self.Theta(stage=j, sys=r)) @ self.prev_fe.Lambda(stage=-1, sys=r) - for r in range(dims.n_sys) - for j in range(opts.n_s) - ]) - elif opts.cross_comp_mode == CrossComplementarityMode.SUM_LAMBDAS_COMPLEMENT_WITH_EVERY_THETA: - # Note: sum_Lambda contains last stage of prev_fe - g_cross_comp = casadi_vertcat_list([ - diag(self.Theta(stage=j, sys=r)) @ self.sum_Lambda(sys=r) - for r in range(dims.n_sys) - for j in range(opts.n_s) - ]) + n = casadi_length(y) - n_cross_comp = casadi_length(g_cross_comp) - g_cross_comp = g_cross_comp - sigma_p - g_cross_comp_ub = 0 * np.ones((n_cross_comp,)) + if opts.mpcc_mode in [MpccMode.SCHOLTES_EQ, MpccMode. SCHOLTES_INEQ]: + # g_comp = diag(y) @ casadi_sum_list([x_i for x_i in x]) - sigma # this works too but is a bit slower. + g_comp = diag(casadi_sum_list([x_i for x_i in x])) @ y - sigma + # NOTE: this line should be equivalent but yield different results + # g_comp = casadi_sum_list([diag(x_i) @ y for x_i in x]) - sigma + elif opts.mpcc_mode == MpccMode.FISCHER_BURMEISTER: + g_comp = SX.zeros(n, 1) + for j in range(n): + for x_i in x: + g_comp[j] += x_i[j] + y[j] - sqrt(x_i[j]**2 + y[j]**2 + sigma**2) + + n_comp = casadi_length(g_comp) if opts.mpcc_mode == MpccMode.SCHOLTES_INEQ: - g_cross_comp_lb = -np.inf * np.ones((n_cross_comp,)) - elif opts.mpcc_mode == MpccMode.SCHOLTES_EQ: - g_cross_comp_lb = 0 * np.ones((n_cross_comp,)) + lb_comp = -np.inf * np.ones((n_comp,)) + ub_comp = 0 * np.ones((n_comp,)) + elif opts.mpcc_mode in [MpccMode.SCHOLTES_EQ, MpccMode.FISCHER_BURMEISTER]: + lb_comp = 0 * np.ones((n_comp,)) + ub_comp = 0 * np.ones((n_comp,)) - self.add_constraint(g_cross_comp, lb=g_cross_comp_lb, ub=g_cross_comp_ub) + self.add_constraint(g_comp, lb=lb_comp, ub=ub_comp) return + def create_complementarity_constraints(self, sigma_p: SX) -> None: + opts = self.opts + if not opts.use_fesd: + for j in range(opts.n_s): + self.create_complementarity([self.Lambda(stage=j)], + self.Theta(stage=j), sigma_p) + elif opts.cross_comp_mode == CrossComplementarityMode.COMPLEMENT_ALL_STAGE_VALUES_WITH_EACH_OTHER: + for j in range(opts.n_s): + # cross comp with prev_fe + self.create_complementarity([self.Theta(stage=j)], + self.prev_fe.Lambda(stage=-1), sigma_p) + for jj in range(opts.n_s): + # within fe + self.create_complementarity([self.Theta(stage=j)], + self.Lambda(stage=jj), sigma_p) + elif opts.cross_comp_mode == CrossComplementarityMode.SUM_LAMBDAS_COMPLEMENT_WITH_EVERY_THETA: + for j in range(opts.n_s): + # Note: sum_Lambda contains last stage of prev_fe + Lambda_list = self.get_Lambdas_incl_last_prev_fe() + self.create_complementarity(Lambda_list, (self.Theta(stage=j)), sigma_p) + return + def step_equilibration(self) -> None: opts = self.opts # step equilibration only within control stages. - if opts.use_fesd and self.fe_idx > 0: - prev_fe: FiniteElement = self.prev_fe - delta_h_ki = self.h() - prev_fe.h() - if opts.step_equilibration == StepEquilibrationMode.HEURISTIC_MEAN: - h_fe = opts.terminal_time / (opts.N_stages * opts.Nfe_list[self.ctrl_idx]) - self.cost += opts.rho_h * (self.h() - h_fe)**2 - elif opts.step_equilibration == StepEquilibrationMode.HEURISTIC_DELTA: - self.cost += opts.rho_h * delta_h_ki**2 - elif opts.step_equilibration == StepEquilibrationMode.L2_RELAXED_SCALED: - eta_k = prev_fe.sum_Lambda() * self.sum_Lambda() + \ - prev_fe.sum_Theta() * self.sum_Theta() - nu_k = 1 - for jjj in range(casadi_length(eta_k)): - nu_k = nu_k * eta_k[jjj] - self.cost += opts.rho_h * tanh(nu_k / opts.step_equilibration_sigma) * delta_h_ki**2 - elif opts.step_equilibration == StepEquilibrationMode.L2_RELAXED: - eta_k = prev_fe.sum_Lambda() * self.sum_Lambda() + \ - prev_fe.sum_Theta() * self.sum_Theta() - nu_k = 1 - for jjj in range(casadi_length(eta_k)): - nu_k = nu_k * eta_k[jjj] - self.cost += opts.rho_h * nu_k * delta_h_ki**2 + if not opts.use_fesd: + return + if not self.fe_idx > 0: + return + + prev_fe: FiniteElement = self.prev_fe + delta_h_ki = self.h() - prev_fe.h() + if opts.step_equilibration == StepEquilibrationMode.HEURISTIC_MEAN: + h_fe = opts.terminal_time / (opts.N_stages * opts.Nfe_list[self.ctrl_idx]) + self.cost += opts.rho_h * (self.h() - h_fe)**2 + return + elif opts.step_equilibration == StepEquilibrationMode.HEURISTIC_DELTA: + self.cost += opts.rho_h * delta_h_ki**2 + return + + # modes that need nu_k + eta_k = prev_fe.sum_Lambda() * self.sum_Lambda() + \ + prev_fe.sum_Theta() * self.sum_Theta() + nu_k = 1 + for jjj in range(casadi_length(eta_k)): + nu_k = nu_k * eta_k[jjj] + + if opts.step_equilibration == StepEquilibrationMode.L2_RELAXED_SCALED: + self.cost += opts.rho_h * tanh(nu_k / opts.step_equilibration_sigma) * delta_h_ki**2 + elif opts.step_equilibration == StepEquilibrationMode.L2_RELAXED: + self.cost += opts.rho_h * nu_k * delta_h_ki**2 + elif opts.step_equilibration == StepEquilibrationMode.DIRECT: + self.add_constraint(nu_k) return @@ -860,7 +899,11 @@ def __init__(self, opts: NosnocOpts, model: NosnocModel, ocp: Optional[NosnocOcp # Scalar-valued complementarity residual if opts.use_fesd: - J_comp = sum1(diag(fe.sum_Theta()) @ fe.sum_Lambda()) + J_comp = 0.0 + for fe in flatten(self.stages): + sum_abs_lam = casadi_sum_list([fabs(lam) for lam in fe.get_Lambdas_incl_last_prev_fe()]) + sum_abs_theta = casadi_sum_list([fabs(t) for t in fe.get_Theta_list()]) + J_comp += sum1(diag(sum_abs_theta) @ sum_abs_lam) else: J_comp = casadi_sum_list([ model.std_compl_res_fun(fe.rk_stage_z(j), fe.p) @@ -876,11 +919,21 @@ def __init__(self, opts: NosnocOpts, model: NosnocModel, ocp: Optional[NosnocOcp self.add_constraint(g_terminal) self.cost += ocp.f_q_T_fun(x_terminal, model.p_ctrl_stages[-1], model.v_global) + # Terminal numerical time if opts.N_stages > 1 and opts.use_fesd: all_h = [fe.h() for stage in self.stages for fe in stage] self.add_constraint(sum(all_h) - opts.terminal_time) + if opts.constraint_handling == ConstraintHandling.LEAST_SQUARES: + for ii in range(casadi_length(self.g)): + if self.lbg[ii] != 0.0: + raise Exception(f"least_squares constraint handling only supported if all lbg, ubg == 0.0, got {self.lbg[ii]=}, {self.ubg[ii]=}, {self.g[ii]=}") + self.cost += self.g[ii] ** 2 + self.g = SX([]) + self.lbg = np.array([]) + self.ubg = np.array([]) + # CasADi Functions self.cost_fun = Function('cost_fun', [self.w], [self.cost]) self.comp_res = Function('comp_res', [self.w, self.p], [J_comp]) diff --git a/nosnoc/nosnoc_opts.py b/nosnoc/nosnoc_opts.py index 1fbecbc..e1dd4e4 100644 --- a/nosnoc/nosnoc_opts.py +++ b/nosnoc/nosnoc_opts.py @@ -4,7 +4,7 @@ from .rk_utils import generate_butcher_tableu, generate_butcher_tableu_integral from .utils import validate -from .nosnoc_types import MpccMode, IrkSchemes, StepEquilibrationMode, CrossComplementarityMode, IrkRepresentation, PssMode, IrkRepresentation, HomotopyUpdateRule, InitializationStrategy +from .nosnoc_types import MpccMode, IrkSchemes, StepEquilibrationMode, CrossComplementarityMode, IrkRepresentation, PssMode, IrkRepresentation, HomotopyUpdateRule, InitializationStrategy, ConstraintHandling @dataclass @@ -26,6 +26,7 @@ class NosnocOpts: irk_scheme: IrkSchemes = IrkSchemes.RADAU_IIA cross_comp_mode: CrossComplementarityMode = CrossComplementarityMode.SUM_LAMBDAS_COMPLEMENT_WITH_EVERY_THETA mpcc_mode: MpccMode = MpccMode.SCHOLTES_INEQ + constraint_handling: ConstraintHandling = ConstraintHandling.EXACT pss_mode: PssMode = PssMode.STEWART # possible options: Stewart and Step gamma_h: float = 1.0 @@ -130,6 +131,14 @@ def preprocess(self): self.right_boundary_point_explicit = True else: self.right_boundary_point_explicit = False + + # checks: + if self.cross_comp_mode == CrossComplementarityMode.SUM_LAMBDAS_COMPLEMENT_WITH_EVERY_THETA and self.mpcc_mode == MpccMode.FISCHER_BURMEISTER: + Warning("UNSUPPORTED option combination comp_mode: SUM_LAMBDAS_COMPLEMENT_WITH_EVERY_THETA and mpcc_mode: MpccMode.FISCHER_BURMEISTER") + if self.mpcc_mode == MpccMode.FISCHER_BURMEISTER and self.constraint_handling != ConstraintHandling.LEAST_SQUARES: + Warning("UNSUPPORTED option combination comp_mode: mpcc_mode == MpccMode.FISCHER_BURMEISTER and constraint_handling != ConstraintHandling.LEAST_SQUARES") + if self.step_equilibration == StepEquilibrationMode.DIRECT and self.constraint_handling != ConstraintHandling.LEAST_SQUARES: + Warning("UNSUPPORTED option combination: StepEquilibrationMode.DIRECT and constraint_handling != ConstraintHandling.LEAST_SQUARES") return ## Options in matlab.. diff --git a/nosnoc/nosnoc_types.py b/nosnoc/nosnoc_types.py index 769f992..1bd7df9 100644 --- a/nosnoc/nosnoc_types.py +++ b/nosnoc/nosnoc_types.py @@ -4,6 +4,8 @@ class MpccMode(Enum): SCHOLTES_INEQ = 0 SCHOLTES_EQ = 1 + FISCHER_BURMEISTER = 2 + # KANZOW_SCHWARTZ = 3 # NOSNOC: 'scholtes_ineq' (3), 'scholtes_eq' (2) # NOTE: tested in simple_sim_tests @@ -27,6 +29,7 @@ class StepEquilibrationMode(Enum): HEURISTIC_DELTA = 1 L2_RELAXED_SCALED = 2 L2_RELAXED = 3 + DIRECT = 4 # NOTE: tested in test_ocp_motor @@ -48,6 +51,10 @@ class HomotopyUpdateRule(Enum): SUPERLINEAR = 1 +class ConstraintHandling(Enum): + EXACT = 0 + LEAST_SQUARES = 1 + class PssMode(Enum): # NOTE: tested in simple_sim_tests, test_ocp_motor STEWART = 0 diff --git a/test/simple_sim_tests.py b/test/simple_sim_tests.py index c1445c2..8698cb9 100644 --- a/test/simple_sim_tests.py +++ b/test/simple_sim_tests.py @@ -106,7 +106,7 @@ def main_test_sliding(): def main_test_discretization(): model = get_simplest_model_sliding() - for mpcc_mode in nosnoc.MpccMode: + for mpcc_mode in [nosnoc.MpccMode.SCHOLTES_EQ, nosnoc.MpccMode.SCHOLTES_INEQ]: for irk_scheme in nosnoc.IrkSchemes: for irk_representation in nosnoc.IrkRepresentation: opts = get_default_options() @@ -147,9 +147,28 @@ def main_test_fesd_off(): print("main_test_fesd_off: SUCCESS") +def main_test_least_squares_problem(): + model = get_simplest_model_switch() + + opts = get_default_options() + opts.print_level = 1 + opts.n_s = 3 + opts.cross_comp_mode = nosnoc.CrossComplementarityMode.COMPLEMENT_ALL_STAGE_VALUES_WITH_EACH_OTHER + opts.mpcc_mode = nosnoc.MpccMode.SCHOLTES_INEQ + opts.mpcc_mode = nosnoc.MpccMode.FISCHER_BURMEISTER + opts.irk_scheme = nosnoc.IrkSchemes.RADAU_IIA + opts.constraint_handling = nosnoc.ConstraintHandling.LEAST_SQUARES + opts.step_equilibration = nosnoc.StepEquilibrationMode.DIRECT + opts.sigma_0 = 0.1 + try: + test_opts(opts, model=model) + except: + raise Exception(f"Test failed with setting:\n {opts=} \n{model=}") + if __name__ == "__main__": test_default() main_test_fesd_off() main_test_discretization() main_test_sliding() main_test_switch() + main_test_least_squares_problem() diff --git a/test/test_ocp_motor.py b/test/test_ocp_motor.py index cd2d9cc..3658114 100644 --- a/test/test_ocp_motor.py +++ b/test/test_ocp_motor.py @@ -17,18 +17,19 @@ def test_loop(): for step_equilibration in nosnoc.StepEquilibrationMode: for pss_mode in nosnoc.PssMode: - opts = get_default_options() - opts.step_equilibration = step_equilibration - opts.pss_mode = pss_mode - # opts.print_level = 0 + if step_equilibration != nosnoc.StepEquilibrationMode.DIRECT: + opts = get_default_options() + opts.step_equilibration = step_equilibration + opts.pss_mode = pss_mode + # opts.print_level = 0 - # print(f"test setting: {opts}") - results = solve_ocp(opts) + # print(f"test setting: {opts}") + results = solve_ocp(opts) - x_traj = results["x_traj"] + x_traj = results["x_traj"] - assert np.allclose(x_traj[0], X0, atol=1e-4) - assert np.allclose(x_traj[-1], X_TARGET, atol=1e-4) + assert np.allclose(x_traj[0], X0, atol=1e-4) + assert np.allclose(x_traj[-1], X_TARGET, atol=1e-4) if __name__ == "__main__":