Correctly add switching constraints to last FE of stage (#49)

Previously we were omitting the switching constraints on the last Finite
Element of a stage. This caused switches at the end of an integration
step to sometimes not correctly be identified.

examples/simplest/simplest_example.py

@@ -22,7 +22,7 @@ def get_default_options():
     return opts
 
 
-def get_simplest_model_sliding():
+def get_simplest_model_sliding(x0=X0):
     # Variable defintion
     x1 = SX.sym("x1")
     x = x1
@@ -31,17 +31,17 @@ def get_simplest_model_sliding():
     # sign matrix for the modes
     S = [np.array([[-1], [1]])]
 
-    f_11 = 3
+    f_11 = 1
     f_12 = -1
     # in matrix form
     F = [horzcat(f_11, f_12)]
 
-    model = nosnoc.NosnocModel(x=x, F=F, S=S, c=c, x0=X0, name='simplest_sliding')
+    model = nosnoc.NosnocModel(x=x, F=F, S=S, c=c, x0=x0, name='simplest_sliding')
 
     return model
 
 
-def get_simplest_model_switch():
+def get_simplest_model_switch(x0=X0):
     # Variable defintion
     x1 = SX.sym("x1")
     x = x1
@@ -60,22 +60,21 @@ def get_simplest_model_switch():
     return model
 
 
-def solve_simplest_example(opts=None, model=None):
+def solve_simplest_example(opts=None, model=None, x0=X0, Nsim=1, Tsim=TSIM):
     if opts is None:
         opts = get_default_options()
         opts.step_equilibration = nosnoc.StepEquilibrationMode.HEURISTIC_MEAN
         opts.pss_mode = nosnoc.PssMode.STEWART
     if model is None:
         model = get_simplest_model_sliding()
 
-    Nsim = 1
-    Tstep = TSIM / Nsim
+    Tstep = Tsim / Nsim
     opts.terminal_time = Tstep
 
     solver = nosnoc.NosnocSolver(opts, model)
 
     # loop
-    looper = nosnoc.NosnocSimLooper(solver, X0, Nsim)
+    looper = nosnoc.NosnocSimLooper(solver, x0, Nsim)
     looper.run()
     results = looper.get_results()
     # solver.print_problem()

nosnoc/problem.py

@@ -473,8 +473,7 @@ def forward_simulation(self, ocp: NosnocOcp, Uk: ca.SX) -> None:
             self.add_constraint(Xk_end - self.w[self.ind_x[-1]])
 
         # g_z_all constraint for boundary point and continuity of algebraic variables.
-        if not opts.right_boundary_point_explicit and opts.use_fesd and (
-                self.fe_idx < opts.Nfe_list[self.ctrl_idx] - 1):
+        if not opts.right_boundary_point_explicit and opts.use_fesd:
             self.add_constraint(
                 model.g_z_switching_fun(self.w[self.ind_x[-1]], self.rk_stage_z(-1), Uk, self.p))
 

test/test_sliding_mode.py

@@ -0,0 +1,68 @@
+import unittest
+from parameterized import parameterized
+import numpy as np
+import nosnoc
+from examples.simplest.simplest_example import (
+    get_default_options,
+    get_simplest_model_sliding,
+    solve_simplest_example
+    )
+IRK_SCHEMES = [
+    nosnoc.IrkSchemes.RADAU_IIA,
+    nosnoc.IrkSchemes.GAUSS_LEGENDRE
+]
+IRK_REPRESENTATIONS = [
+    nosnoc.IrkRepresentation.DIFFERENTIAL,
+    nosnoc.IrkRepresentation.DIFFERENTIAL_LIFT_X,
+    nosnoc.IrkRepresentation.INTEGRAL
+]
+
+PSS_MODES = [
+    nosnoc.PssMode.STEP,
+    nosnoc.PssMode.STEWART
+]
+
+CROSS_COMP_MODES = [
+    nosnoc.CrossComplementarityMode.COMPLEMENT_ALL_STAGE_VALUES_WITH_EACH_OTHER,
+    nosnoc.CrossComplementarityMode.SUM_LAMBDAS_COMPLEMENT_WITH_EVERY_THETA
+]
+
+options = [
+    (irk_scheme, irk_representation, pss_mode, cross_comp_mode)
+    for irk_scheme in IRK_SCHEMES
+    for irk_representation in IRK_REPRESENTATIONS
+    for pss_mode in PSS_MODES
+    for cross_comp_mode in CROSS_COMP_MODES
+]
+
+
+class TestSlidingMode(unittest.TestCase):
+
+    @parameterized.expand(options)
+    def test_combination(self, irk_scheme, irk_representation, pss_mode, cross_comp_mode):
+        opts = get_default_options()
+        opts.comp_tol = 1e-7
+        opts.irk_representation = irk_representation
+        opts.irk_scheme = irk_scheme
+        opts.pss_mode = pss_mode
+        opts.cross_comp_mode = cross_comp_mode
+
+        message = (
+            f"Test setting:\n{irk_representation}\n{irk_scheme}\n{pss_mode}\n{cross_comp_mode}"
+        )
+        print(message)
+        x0 = np.array([-np.sqrt(2)])
+        results = solve_simplest_example(opts, get_simplest_model_sliding(x0), x0=x0, Nsim=7, Tsim=2.0)
+
+        x_traj = results["X_sim"]
+        t_grid = results["t_grid"]
+        print(x_traj)
+        print(t_grid)
+        xt = np.vstack((x_traj.T, t_grid))
+        diff = xt - np.array([[0], [np.sqrt(2)]])
+
+        self.assertTrue(np.min(np.linalg.norm(diff, axis=0)) < 1e-5, message)
+
+
+if __name__ == "__main__":
+    unittest.main()