From 9a88e00f16ef5b2794882d8d2943116910c5ee3f Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Wed, 22 Jul 2026 10:05:32 -0400 Subject: [PATCH] fix: scale and order Acados control bounds --- bioptim/interfaces/acados_interface.py | 23 ++------- bioptim/interfaces/acados_utils.py | 17 +++++++ tests/shard1/test_acados_control_bounds.py | 59 ++++++++++++++++++++++ 3 files changed, 81 insertions(+), 18 deletions(-) create mode 100644 bioptim/interfaces/acados_utils.py create mode 100644 tests/shard1/test_acados_control_bounds.py diff --git a/bioptim/interfaces/acados_interface.py b/bioptim/interfaces/acados_interface.py index a7517996b..37ef05bc2 100644 --- a/bioptim/interfaces/acados_interface.py +++ b/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]) diff --git a/bioptim/interfaces/acados_utils.py b/bioptim/interfaces/acados_utils.py new file mode 100644 index 000000000..e87680911 --- /dev/null +++ b/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 diff --git a/tests/shard1/test_acados_control_bounds.py b/tests/shard1/test_acados_control_bounds.py new file mode 100644 index 000000000..22caea23a --- /dev/null +++ b/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)