Fixed MHE

bioptim/examples/moving_horizon_estimation/cyclic_nmpc.py

@@ -69,7 +69,7 @@ def prepare_nmpc(
         dynamics,
         cycle_len,
         cycle_duration,
-        objective_functions=new_objectives,
+        common_objective_functions=new_objectives,
         constraints=constraints,
         x_bounds=x_bound,
         u_bounds=u_bound,

bioptim/examples/moving_horizon_estimation/mhe.py

@@ -155,7 +155,7 @@ def prepare_mhe(
         Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics),
         window_len,
         window_duration,
-        objective_functions=new_objectives,
+        common_objective_functions=new_objectives,
         x_bounds=x_bounds,
         u_bounds=u_bounds,
         x_init=x_init_list,

bioptim/optimization/optimal_control_program.py

@@ -134,8 +134,6 @@ class OptimalControlProgram:
         Set the internal stochastic variables (s_init, s_bounds, s_scaling) if any of the phases is stochastic
     _check_quaternions_hasattr(self, bio_model)
         Check if the bio_model has quaternions and set the flag accordingly
-    _check_and_prepare_dynamics(self, dynamics)
-        Check if the dynamics is a Dynamics or a DynamicsList and set the flag accordingly
     """
 
     def __init__(
@@ -236,7 +234,6 @@ def __init__(
         bio_model = self._initialize_model(bio_model)
 
         # Placed here because of MHE
-        self._check_and_prepare_dynamics(dynamics)
         s_init, s_bounds, s_scaling = self._set_stochastic_internal_stochastic_variables()
 
         self._check_and_set_threads(n_threads)
@@ -337,17 +334,7 @@ def _initialize_model(self, bio_model):
         bio_model = self._check_quaternions_hasattr(bio_model)
         self.n_phases = len(bio_model)
         return bio_model
-
-    def _check_and_prepare_dynamics(self, dynamics):
-        if isinstance(dynamics, Dynamics):
-            dynamics_type_tp = DynamicsList()
-            dynamics_type_tp.add(dynamics)
-            self.dynamics = dynamics_type_tp
-        elif isinstance(dynamics, DynamicsList):
-            self.dynamics = dynamics
-        elif not isinstance(dynamics, DynamicsList):
-            raise RuntimeError("dynamics should be a Dynamics or a DynamicsList")
-
+
     def _check_and_set_threads(self, n_threads):
         if not isinstance(n_threads, int) or isinstance(n_threads, bool) or n_threads < 1:
             raise RuntimeError("n_threads should be a positive integer greater or equal than 1")

bioptim/optimization/receding_horizon_optimization.py

@@ -3,6 +3,7 @@
 from typing import Callable
 from time import perf_counter
 
+from casadi import SX
 import numpy as np
 
 from .optimal_control_program import OptimalControlProgram
@@ -35,6 +36,7 @@ def __init__(
         dynamics: Dynamics | DynamicsList,
         window_len: int | list | tuple,
         window_duration: int | float | list | tuple,
+        common_objective_functions: ObjectiveList = None,
         use_sx=True,
         **kwargs,
     ):
@@ -49,19 +51,30 @@ def __init__(
             The length of the sliding window. It is translated into n_shooting in each individual optimization program
         window_duration
             The time in second of the sliding window
+        common_objective_functions
+            The objective functions that carries through all the individual optimization program
         use_sx
             Same as OCP, but has True as default value
         """
 
         if isinstance(bio_model, (list, tuple)) and len(bio_model) > 1:
             raise ValueError("Receding horizon optimization must be defined using only one bio_model")
 
+        if "objective_functions" in kwargs:
+            raise ValueError(
+                "'objective_functions' should be defined via 'common_objective_functions' for the objectives that are shared between the windows "
+                "or via 'update_objectives' for the objective that is specific to each window"
+            )
+
+        self.common_objective_functions = deepcopy(common_objective_functions)
+
         super(RecedingHorizonOptimization, self).__init__(
             bio_model=bio_model,
             dynamics=dynamics,
             n_shooting=window_len,
             phase_time=window_duration,
             use_sx=use_sx,
+            objective_functions=self.common_objective_functions,
             **kwargs,
         )
         self.total_optimization_run = 0
@@ -217,23 +230,24 @@ def _initialize_solution(self, states: list, controls: list):
         for key in self.nlp[0].controls.keys():
             controls_tp = np.concatenate([control[key] for control in controls], axis=1)
             u_init_for_solution.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME)
-            if self.original_values["control_type"] == ControlType.CONSTANT:
+            if self.nlp[0].control_type == ControlType.CONSTANT:
                 controls_tp = controls_tp[:, :-1]
             u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME)
 
-        model_class = self.original_values["bio_model"][0][0]
-        model_initializer = self.original_values["bio_model"][0][1]
+        model_serialized = self.nlp[0].model.serialize()
+        model_class = model_serialized[0]
+        model_initializer = model_serialized[1]
         solution_ocp = OptimalControlProgram(
             bio_model=model_class(**model_initializer),
-            dynamics=self.original_values["dynamics"][0],
+            dynamics=self.nlp[0].dynamics_type,
             ode_solver=self.nlp[0].ode_solver,
             n_shooting=self.total_optimization_run - 1,
             phase_time=self.total_optimization_run * self.nlp[0].dt,
             x_bounds=self.nlp[0].x_bounds,
             u_bounds=self.nlp[0].u_bounds,
             x_init=x_init,
             u_init=u_init,
-            use_sx=self.original_values["use_sx"],
+            use_sx=self.cx == SX,
         )
         s_init = InitialGuessList()
         p_init = InitialGuessList()
@@ -439,21 +453,23 @@ def _initialize_solution(self, states: list, controls: list):
         for key in self.nlp[0].controls.keys():
             controls_tp = np.concatenate([control[key] for control in controls], axis=1)
             u_init_for_solution.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME)
-            if self.original_values["control_type"] == ControlType.CONSTANT:
+            if self.nlp[0].control_type == ControlType.CONSTANT:
                 controls_tp = controls_tp[:, :-1]
             u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME)
-        model_class = self.original_values["bio_model"][0][0]
-        model_initializer = self.original_values["bio_model"][0][1]
+
+        model_serialized = self.nlp[0].model.serialize()
+        model_class = model_serialized[0]
+        model_initializer = model_serialized[1]
         solution_ocp = OptimalControlProgram(
             bio_model=model_class(**model_initializer),
-            dynamics=self.original_values["dynamics"][0],
+            dynamics=self.nlp[0].dynamics_type,
             n_shooting=self.total_optimization_run * self.nlp[0].ns - 1,
             phase_time=self.total_optimization_run * self.nlp[0].ns * self.nlp[0].dt,
             x_bounds=self.nlp[0].x_bounds,
             u_bounds=self.nlp[0].u_bounds,
             x_init=x_init,
             u_init=u_init,
-            use_sx=self.original_values["use_sx"],
+            use_sx=self.cx == SX,
         )
         s_init = InitialGuessList()
         p_init = InitialGuessList()
@@ -673,22 +689,23 @@ def _initialize_solution(self, states: list, controls: list):
         for key in self.nlp[0].controls.keys():
             controls_tp = np.concatenate([control[key] for control in controls], axis=1)
             u_init_for_solution.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME, phase=0)
-            if self.original_values["control_type"] == ControlType.CONSTANT:
+            if self.nlp[0].control_type == ControlType.CONSTANT:
                 controls_tp = controls_tp[:, :-1]
             u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME, phase=0)
 
-        model_class = self.original_values["bio_model"][0][0]
-        model_initializer = self.original_values["bio_model"][0][1]
+        model_serialized = self.nlp[0].model.serialize()
+        model_class = model_serialized[0]
+        model_initializer = model_serialized[1]
         solution_ocp = OptimalControlProgram(
             bio_model=model_class(**model_initializer),
-            dynamics=self.original_values["dynamics"][0],
+            dynamics=self.nlp[0].dynamics_type,
             ode_solver=self.nlp[0].ode_solver,
-            objective_functions=deepcopy(self.original_values["objective_functions"]),
+            objective_functions=deepcopy(self.common_objective_functions),
             n_shooting=self.cycle_len * self.total_optimization_run - 1,
             phase_time=(self.cycle_len * self.total_optimization_run - 1) * self.nlp[0].dt,
             x_init=x_init,
             u_init=u_init,
-            use_sx=self.original_values["use_sx"],
+            use_sx=self.cx == SX,
         )
         s_init = InitialGuessList()
         p_init = InitialGuessList()
@@ -710,25 +727,23 @@ def _initialize_one_cycle(self, states: np.ndarray, controls: np.ndarray):
         for key in self.nlp[0].controls.keys():
             controls_tp = controls[key]
             u_init_for_solution.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME, phase=0)
-            if self.original_values["control_type"] == ControlType.CONSTANT:
+            if self.nlp[0].control_type == ControlType.CONSTANT:
                 controls_tp = controls_tp[:, :-1]
             u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME, phase=0)
 
-        original_values = self.original_values
-
-        model_class = original_values["bio_model"][0][0]
-        model_initializer = original_values["bio_model"][0][1]
-
+        model_serialized = self.nlp[0].model.serialize()
+        model_class = model_serialized[0]
+        model_initializer = model_serialized[1]
         solution_ocp = OptimalControlProgram(
             bio_model=model_class(**model_initializer),
-            dynamics=original_values["dynamics"][0],
-            objective_functions=deepcopy(original_values["objective_functions"]),
+            dynamics=self.nlp[0].dynamics_type,
+            objective_functions=deepcopy(self.common_objective_functions),
             ode_solver=self.nlp[0].ode_solver,
             n_shooting=self.cycle_len,
             phase_time=self.cycle_len * self.nlp[0].dt,
             x_init=x_init,
             u_init=u_init,
-            use_sx=original_values["use_sx"],
+            use_sx=self.cx == SX,
         )
         s_init = InitialGuessList()
         p_init = InitialGuessList()