fix issues + API wip

- changes in API (tentative)
- fix issues with double-printing log msg
- retry warnings suppressed
- updated unit tests
- new unit test in CI

Author: alphaville

ci/script.sh

@@ -52,6 +52,7 @@ regular_test() {
     export PYTHONPATH=.
     python -W ignore test/test_constraints.py -v
     python -W ignore test/test.py -v
+    python -W ignore test/test_ocp.py -v
     python -W ignore test/test_raspberry_pi.py -v
 
 

open-codegen/opengen/builder/optimizer_builder.py

@@ -86,6 +86,9 @@ def with_verbosity_level(self, verbosity_level):
         self.__logger.setLevel(verbosity_level)
         self.__logger.handlers.clear()
         self.__logger.addHandler(stream_handler)
+        # Keep logs on this named logger only; otherwise callers that configure
+        # the root logger can end up seeing every builder message twice.
+        self.__logger.propagate = False
 
         return self
 

open-codegen/opengen/ocp/builder.py

@@ -161,6 +161,55 @@ def __make_multiple_shooting_constraints(self):
         if stage_constraints is None:
             stage_constraints = NoConstraints()
 
+        hard_stage_constraints = self.__ocp.hard_stage_state_input_constraints
+        hard_terminal_constraints = self.__ocp.hard_terminal_state_constraints
+
+        if hard_stage_constraints is not None and self.__ocp.input_constraints is not None:
+            raise ValueError(
+                "cannot combine with_input_constraints with "
+                "with_hard_stage_state_input_constraints automatically; "
+                "encode the input bounds directly in the stage state-input set"
+            )
+
+        if hard_stage_constraints is not None and hard_terminal_constraints is not None:
+            raise ValueError(
+                "cannot combine with_hard_stage_state_input_constraints with "
+                "with_hard_terminal_state_constraints automatically; "
+                "their intersection on the terminal stage is not supported"
+            )
+
+        if hard_stage_constraints is not None:
+            segments = [
+                ((idx + 1) * (self.__ocp.nu + self.__ocp.nx)) - 1
+                for idx in range(self.__ocp.horizon)
+            ]
+            constraints = [hard_stage_constraints] * self.__ocp.horizon
+            return CartesianProduct(segments, constraints)
+
+        if hard_terminal_constraints is not None:
+            segments = []
+            constraints = []
+            offset = -1
+
+            for _ in range(self.__ocp.horizon - 1):
+                offset += self.__ocp.nu
+                segments.append(offset)
+                constraints.append(stage_constraints)
+
+                offset += self.__ocp.nx
+                segments.append(offset)
+                constraints.append(NoConstraints())
+
+            offset += self.__ocp.nu
+            segments.append(offset)
+            constraints.append(stage_constraints)
+
+            offset += self.__ocp.nx
+            segments.append(offset)
+            constraints.append(hard_terminal_constraints)
+
+            return CartesianProduct(segments, constraints)
+
         segments = []
         constraints = []
         offset = -1

open-codegen/opengen/ocp/problem.py

@@ -39,6 +39,8 @@ def __init__(self, nx, nu, horizon, shooting=ShootingMethod.SINGLE):
         self.__stage_cost = None
         self.__terminal_cost = None
         self.__input_constraints = None
+        self.__hard_stage_state_input_constraints = None
+        self.__hard_terminal_state_constraints = None
         self.__path_constraints = []
         self.__terminal_constraints = []
 
@@ -74,6 +76,14 @@ def dynamics(self):
     def input_constraints(self):
         return self.__input_constraints
 
+    @property
+    def hard_stage_state_input_constraints(self):
+        return self.__hard_stage_state_input_constraints
+
+    @property
+    def hard_terminal_state_constraints(self):
+        return self.__hard_terminal_state_constraints
+
     @property
     def path_constraints(self):
         return list(self.__path_constraints)
@@ -108,6 +118,14 @@ def with_input_constraints(self, constraints):
         self.__input_constraints = constraints
         return self
 
+    def with_hard_stage_state_input_constraints(self, constraints):
+        self.__hard_stage_state_input_constraints = constraints
+        return self
+
+    def with_hard_terminal_state_constraints(self, constraints):
+        self.__hard_terminal_state_constraints = constraints
+        return self
+
     @staticmethod
     def __constraint_dimension(mapping):
         return mapping.size1() * mapping.size2()
@@ -166,6 +184,15 @@ def validate(self):
             raise ValueError("dynamics must be specified")
         if self.__stage_cost is None:
             raise ValueError("stage cost must be specified")
+        if self.__shooting == ShootingMethod.SINGLE:
+            if self.__hard_stage_state_input_constraints is not None:
+                raise ValueError(
+                    "with_hard_stage_state_input_constraints is only available in multiple shooting"
+                )
+            if self.__hard_terminal_state_constraints is not None:
+                raise ValueError(
+                    "with_hard_terminal_state_constraints is only available in multiple shooting"
+                )
         return self
 
     def __build_single_shooting_model(self):
@@ -264,6 +291,7 @@ def __build_multiple_shooting_model(self):
         cost = 0
         penalty_terms = []
         alm_blocks = []
+        dynamics_alm_terms = []
 
         x_current = x0
         offset = 0
@@ -296,19 +324,23 @@ def __build_multiple_shooting_model(self):
             if self.__dynamics_constraint_kind == "penalty":
                 penalty_terms.append(dynamics_defect)
             else:
-                alm_blocks.append({
-                    "mapping": dynamics_defect,
-                    "dimension": self.__constraint_dimension(dynamics_defect),
-                    "set_c": Zero(),
-                    "set_y": None,
-                })
+                dynamics_alm_terms.append(dynamics_defect)
 
             x_current = x_next
             states.append(x_current)
 
         if self.__terminal_cost is not None:
             cost += self.__terminal_cost(x_current, param)
 
+        if dynamics_alm_terms:
+            dynamics_mapping = cs.vertcat(*dynamics_alm_terms)
+            alm_blocks.append({
+                "mapping": dynamics_mapping,
+                "dimension": self.__constraint_dimension(dynamics_mapping),
+                "set_c": Zero(),
+                "set_y": None,
+            })
+
         for definition in self.__terminal_constraints:
             mapping = definition["constraint"](x_current, param)
             if definition["kind"] == "penalty":

open-codegen/opengen/tcp/optimizer_tcp_manager.py

@@ -11,6 +11,9 @@
 from .solver_response import SolverResponse
 from importlib.metadata import version
 
+# A bit of warning suppressing for the retry module
+logging.getLogger("retry").setLevel(logging.ERROR)
+
 class OptimizerTcpManager:
     """Client for TCP interface of parametric optimizers
 

open-codegen/test/test_ocp.py

@@ -64,6 +64,21 @@ def test_parameter_defaults_are_packed(self):
         packed = ocp.parameters.pack({"x0": [0.5, -0.25]})
         self.assertEqual(packed, [0.5, -0.25, 1.0, -1.0])
 
+    def test_single_shooting_rejects_hard_state_based_constraints(self):
+        ocp_stage = self.make_ocp()
+        ocp_stage.with_hard_stage_state_input_constraints(
+            og.constraints.Rectangle([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0])
+        )
+        with self.assertRaises(ValueError):
+            ocp_stage.build_symbolic_model()
+
+        ocp_terminal = self.make_ocp()
+        ocp_terminal.with_hard_terminal_state_constraints(
+            og.constraints.Rectangle([-1.0, -1.0], [1.0, 1.0])
+        )
+        with self.assertRaises(ValueError):
+            ocp_terminal.build_symbolic_model()
+
     def test_symbolic_lowering_builds_penalty_mapping_and_cartesian_constraints(self):
         ocp = self.make_ocp()
         builder = og.ocp.OCPBuilder(
@@ -123,12 +138,13 @@ def test_generated_optimizer_uses_named_parameters_and_defaults(self):
         )
 
         result = optimizer.solve(x0=[0.0, 0.0])
-        print(result)
+        # print(result)
 
         self.assertEqual(backend.last_call["p"], [0.0, 0.0, 1.0, -1.0])
         self.assertEqual(result.inputs, [[0.1], [0.2], [0.3]])
         self.assertEqual(result.states[0], [0.0, 0.0])
-        self.assertEqual(result.states[-1], [0.6000000000000001, -0.6000000000000001])
+        self.assertAlmostEqual(result.states[-1][0], 0.6)
+        self.assertAlmostEqual(result.states[-1][1], -0.6)
 
     def test_multiple_shooting_builds_decision_vector_and_defect_constraints(self):
         ocp = self.make_ocp(shooting=og.ocp.ShootingMethod.MULTIPLE)
@@ -156,8 +172,23 @@ def test_multiple_shooting_can_impose_dynamics_with_alm(self):
         self.assertEqual(low_level.dim_decision_variables(), 9)
         self.assertEqual(low_level.dim_constraints_penalty(), 3)
         self.assertEqual(low_level.dim_constraints_aug_lagrangian(), 6)
-        self.assertIsInstance(low_level.alm_set_c, og.constraints.CartesianProduct)
-        self.assertEqual(low_level.alm_set_c.segments, [1, 3, 5])
+        self.assertIsInstance(low_level.alm_set_c, og.constraints.Zero)
+        self.assertIsInstance(low_level.alm_set_y, og.constraints.BallInf)
+
+    def test_multiple_shooting_supports_hard_terminal_state_constraints(self):
+        ocp = self.make_ocp(shooting=og.ocp.ShootingMethod.MULTIPLE)
+        ocp.with_hard_terminal_state_constraints(
+            og.constraints.Rectangle([-10.0, -10.0], [10.0, 10.0])
+        )
+        builder = og.ocp.OCPBuilder(
+            ocp,
+            metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_ms_terminal_hard"),
+        )
+        low_level = builder.build_problem()
+
+        self.assertIsInstance(low_level.constraints, og.constraints.CartesianProduct)
+        self.assertEqual(low_level.constraints.segments, [0, 2, 3, 5, 6, 8])
+        self.assertIsInstance(low_level.constraints.constraints[-1], og.constraints.Rectangle)
 
     def test_multiple_shooting_solution_extracts_states_from_decision_vector(self):
         ocp = self.make_ocp(shooting=og.ocp.ShootingMethod.MULTIPLE)
@@ -179,38 +210,28 @@ def test_multiple_shooting_solution_extracts_states_from_decision_vector(self):
         self.assertEqual(result.inputs, [[0.1], [0.2], [0.4]])
         self.assertEqual(result.states, [[0.0, 0.0], [0.1, -0.1], [0.3, -0.3], [0.7, -0.7]])
 
-    def test_ocp_generates_rust_solver_and_calls_tcp_interface(self):
-        optimizer_name = "ocp_tcp_smoke"
-        port = 3391
-
-        ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=5,
-                                           shooting=og.ocp.ShootingMethod.MULTIPLE)
-        ocp.add_parameter("x0", 2)
-        ocp.add_parameter("xref", 2, default=[0.0, 0.0])
-        ocp.with_dynamics(lambda x, u, param: cs.vertcat(x[0] + u[0], 
-                                                         x[1] - u[0]))
-        ocp.with_dynamics_constraints("alm")
-        ocp.with_stage_cost(
-            lambda x, u, param, _t: 
-            cs.exp( cs.dot(x - param["xref"], x - param["xref"]) + 0.01 * cs.dot(u, u))
-        )
-        ocp.with_terminal_cost(
-            lambda x, param: 
-            2.0 * cs.dot(x - param["xref"], x - param["xref"])
-        )
-        ocp.with_input_constraints(og.constraints.Rectangle([-0.4], [0.4]))
-        ocp.with_path_constraint(
-            lambda x, u, param, _t: cs.fmax(0.0, x[0] - 1.5)
-        )
-
-        meta = og.config.OptimizerMeta().with_optimizer_name(optimizer_name)
-        build_config = og.config.BuildConfiguration() \
-            .with_open_version(local_path=OcpTestCase.get_open_local_absolute_path()) \
-            .with_build_directory(OcpTestCase.TEST_DIR) \
-            .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE) \
-            .with_tcp_interface_config(
-                tcp_interface_config=og.config.TcpServerConfiguration(bind_port=port)
+    def test_single_and_multiple_shooting_give_approximately_equal_results(self):
+        def make_problem(shooting):
+            ocp = og.ocp.OptimalControlProblem(nx=2, nu=1, horizon=5, shooting=shooting)
+            ocp.add_parameter("x0", 2)
+            ocp.add_parameter("xref", 2, default=[0.0, 0.0])
+            ocp.with_dynamics(lambda x, u, param: cs.vertcat(x[0] + u[0], x[1] - u[0]))
+            ocp.with_stage_cost(
+                lambda x, u, param, _t:
+                cs.dot(x - param["xref"], x - param["xref"]) + 0.01 * cs.dot(u, u)
             )
+            ocp.with_terminal_cost(
+                lambda x, param:
+                2.0 * cs.dot(x - param["xref"], x - param["xref"])
+            )
+            ocp.with_input_constraints(og.constraints.Rectangle([-0.4], [0.4]))
+            ocp.with_path_constraint(
+                lambda x, u, param, _t: cs.fmax(0.0, x[0] - 1.5)
+            )
+            if shooting == og.ocp.ShootingMethod.MULTIPLE:
+                ocp.with_dynamics_constraints("alm")
+            return ocp
+
         solver_config = og.config.SolverConfiguration() \
             .with_tolerance(1e-5) \
             .with_delta_tolerance(1e-5) \
@@ -219,23 +240,59 @@ def test_ocp_generates_rust_solver_and_calls_tcp_interface(self):
             .with_max_inner_iterations(5000) \
             .with_max_outer_iterations(20)
 
-        builder = og.ocp.OCPBuilder(
-            ocp,
-            metadata=meta,
-            build_configuration=build_config,
+        single_optimizer = og.ocp.OCPBuilder(
+            make_problem(og.ocp.ShootingMethod.SINGLE),
+            metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_single_tcp"),
+            build_configuration=og.config.BuildConfiguration()
+                .with_open_version(local_path=OcpTestCase.get_open_local_absolute_path())
+                .with_build_directory(OcpTestCase.TEST_DIR)
+                .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE)
+                .with_tcp_interface_config(
+                    tcp_interface_config=og.config.TcpServerConfiguration(bind_port=3391)
+                ),
             solver_configuration=solver_config,
-        )
-        optimizer = builder.build()
+        ).build()
+        multiple_optimizer = og.ocp.OCPBuilder(
+            make_problem(og.ocp.ShootingMethod.MULTIPLE),
+            metadata=og.config.OptimizerMeta().with_optimizer_name("ocp_multiple_tcp"),
+            build_configuration=og.config.BuildConfiguration()
+                .with_open_version(local_path=OcpTestCase.get_open_local_absolute_path())
+                .with_build_directory(OcpTestCase.TEST_DIR)
+                .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE)
+                .with_tcp_interface_config(
+                    tcp_interface_config=og.config.TcpServerConfiguration(bind_port=3392)
+                ),
+            solver_configuration=solver_config,
+        ).build()
 
         try:
-            result = optimizer.solve(x0=[1.0, -1.0], xref=[0.0, 0.0])
-            print(result)
-            self.assertEqual("Converged", result.exit_status)
-            self.assertEqual(15, len(result.solution))
-            self.assertEqual(5, len(result.inputs))
-            self.assertEqual(6, len(result.states))
+            x0 = [1.0, -1.0]
+            xref = [0.0, 0.0]
+            single_result = single_optimizer.solve(x0=x0, xref=xref)
+            multiple_result = multiple_optimizer.solve(x0=x0, xref=xref)
+            # print("SINGLE\n------------")
+            # print(single_result)
+            # print("MULTIPLE\n------------")
+            # print(multiple_result)
+
+            self.assertEqual("Converged", single_result.exit_status)
+            self.assertEqual("Converged", multiple_result.exit_status)
+            self.assertEqual(5, len(single_result.inputs))
+            self.assertEqual(5, len(multiple_result.inputs))
+            self.assertEqual(6, len(single_result.states))
+            self.assertEqual(6, len(multiple_result.states))
+
+            for u_single, u_multiple in zip(single_result.inputs, multiple_result.inputs):
+                self.assertAlmostEqual(u_single[0], u_multiple[0], delta=1e-3)
+
+            for x_single, x_multiple in zip(single_result.states, multiple_result.states):
+                self.assertAlmostEqual(x_single[0], x_multiple[0], delta=1e-3)
+                self.assertAlmostEqual(x_single[1], x_multiple[1], delta=1e-3)
+
+            self.assertAlmostEqual(single_result.cost, multiple_result.cost, delta=1e-3)
         finally:
-            optimizer.kill()
+            single_optimizer.kill()
+            multiple_optimizer.kill()
 
 
 if __name__ == "__main__":