[Wave RHO][1/4] Fix Acados control-bound scaling and ordering

bioptim/interfaces/acados_interface.py

@@ -7,6 +7,7 @@
 from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 
 from .solver_interface import SolverInterface
+from .acados_utils import scaled_control_bounds
 from ..interfaces import Solver
 from ..misc.enums import Node, SolverType, PhaseDynamics
 from ..limits.objective_functions import ObjectiveFunction, ObjectiveFcn
@@ -368,16 +369,9 @@ def __set_constraints(self, ocp) -> None:
                 self.x_bound_min[index, i] = x_tp.min[:, i]
 
         # setup control constraints
-        u_bounds_max = np.ndarray((self.acados_ocp.dims.nu, 1))
-        u_bounds_min = np.ndarray((self.acados_ocp.dims.nu, 1))
-        for key in ocp.nlp[0].controls.keys():
-            u_tp = ocp.nlp[0].u_bounds[key].scale(ocp.nlp[0].u_scaling[key].scaling)
-            index = ocp.nlp[0].controls[key].index
-            u_bounds_max[index, 0] = np.array(u_tp.max[:, 0])
-            u_bounds_min[index, 0] = np.array(u_tp.min[:, 0])
-
-        self.acados_ocp.constraints.lbu = u_bounds_max
-        self.acados_ocp.constraints.ubu = u_bounds_min
+        u_bounds_min, u_bounds_max = scaled_control_bounds(ocp.nlp[0])
+        self.acados_ocp.constraints.lbu = u_bounds_min[:, np.newaxis]
+        self.acados_ocp.constraints.ubu = u_bounds_max[:, np.newaxis]
         self.acados_ocp.constraints.idxbu = np.array(range(self.acados_ocp.dims.nu))
         self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
 
@@ -808,14 +802,7 @@ def __update_solver(self):
             self.ocp_solver.set(n, "u", u_init)
 
             # The u_bounds need to be ordered by index that's why we use a for loop
-            u_bounds_max = np.ndarray(self.acados_ocp.dims.nu)
-            u_bounds_min = np.ndarray(self.acados_ocp.dims.nu)
-            for key in self.ocp.nlp[0].controls.keys():
-                u_tp = self.ocp.nlp[0].u_bounds[key]
-                index = self.ocp.nlp[0].controls[key].index
-                u_bounds_max[index] = np.array(u_tp.max[:, 0])
-                u_bounds_min[index] = np.array(u_tp.min[:, 0])
-
+            u_bounds_min, u_bounds_max = scaled_control_bounds(self.ocp.nlp[0])
             self.ocp_solver.constraints_set(n, "lbu", u_bounds_min)
             self.ocp_solver.constraints_set(n, "ubu", u_bounds_max)
             self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:, 0])

bioptim/interfaces/acados_utils.py

@@ -0,0 +1,17 @@
+import numpy as np
+
+
+def scaled_control_bounds(nlp) -> tuple[np.ndarray, np.ndarray]:
+    """Assemble Acados control bounds in solver order and scaled coordinates."""
+
+    lower = np.empty(nlp.controls.shape)
+    upper = np.empty(nlp.controls.shape)
+    for key in nlp.controls.keys():
+        bounds = nlp.u_bounds[key].scale(nlp.u_scaling[key].scaling)
+        index = nlp.controls[key].index
+        lower[index] = np.asarray(bounds.min[:, 0], dtype=float)
+        upper[index] = np.asarray(bounds.max[:, 0], dtype=float)
+
+    if np.any(lower > upper):
+        raise ValueError(f"Scaled Acados control bounds are inconsistent: lower={lower}, upper={upper}")
+    return lower, upper

tests/shard1/test_acados_control_bounds.py

@@ -0,0 +1,59 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from bioptim.interfaces.acados_utils import scaled_control_bounds
+from bioptim.limits.path_conditions import Bounds
+from bioptim.misc.enums import InterpolationType
+
+
+class _Variables(dict):
+    @property
+    def shape(self):
+        return sum(len(variable.index) for variable in self.values())
+
+
+def test_scaled_control_bounds_keep_lower_upper_order_and_variable_indices():
+    controls = _Variables(
+        pulse=SimpleNamespace(index=[1]),
+        tau=SimpleNamespace(index=[0, 2]),
+    )
+    nlp = SimpleNamespace(
+        controls=controls,
+        u_bounds={
+            "pulse": Bounds(
+                "pulse", [20.0], [60.0], interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT
+            ),
+            "tau": Bounds(
+                "tau",
+                [-10.0, -30.0],
+                [10.0, 50.0],
+                interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
+            ),
+        },
+        u_scaling={
+            "pulse": SimpleNamespace(scaling=np.array([[10.0]])),
+            "tau": SimpleNamespace(scaling=np.array([[2.0], [10.0]])),
+        },
+    )
+
+    lower, upper = scaled_control_bounds(nlp)
+
+    np.testing.assert_allclose(lower, [-5.0, 2.0, -3.0])
+    np.testing.assert_allclose(upper, [5.0, 6.0, 5.0])
+    assert np.all(lower <= upper)
+
+
+def test_scaled_control_bounds_reject_inverted_bounds():
+    controls = _Variables(u=SimpleNamespace(index=[0]))
+    nlp = SimpleNamespace(
+        controls=controls,
+        u_bounds={
+            "u": Bounds("u", [2.0], [1.0], interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
+        },
+        u_scaling={"u": SimpleNamespace(scaling=np.array([[1.0]]))},
+    )
+
+    with pytest.raises(ValueError, match="inconsistent"):
+        scaled_control_bounds(nlp)