Merge pull request #17 from Kev1CO/simulation_processing

Adding moving time horizon for cycling tasks

Author: Kev1CO

.gitignore

@@ -5,17 +5,10 @@ __pycache__/
 
 .pytest_cache
 
-
-examples/data/debug.py
-
 external/bioptim
 
 example/*.pkl
-example/fes_multibody_cycling/*.pkl
-example/reaching_task/*.pkl
-example/minimize_fatigue/*.pkl
-
-example/minimize_fatigue/result_file/*.pkl
-examples/nmpc/results/*.pkl
+**/cycling/plot/
+**/cycling/result/
 
 *.pkl

README.md

@@ -24,8 +24,12 @@ def minimize_overall_muscle_fatigue(controller: PenaltyController) -> MX:
         The sum of each force scaling factor
         """
         muscle_name_list = controller.model.bio_model.muscle_names
+        muscle_model = controller.model.muscles_dynamics_model
         muscle_fatigue = vertcat(
-            *[controller.states["A_" + muscle_name_list[x]].cx for x in range(len(muscle_name_list))]
+            *[
+                1 - (controller.states["A_" + muscle_name_list[x]].cx / muscle_model[x].a_scale)
+                for x in range(len(muscle_name_list))
+            ]
         )
         return muscle_fatigue
 
@@ -44,8 +48,12 @@ def minimize_overall_muscle_force_production(controller: PenaltyController) -> M
         The sum of each force
         """
         muscle_name_list = controller.model.bio_model.muscle_names
+        muscle_model = controller.model.muscles_dynamics_model
         muscle_force = vertcat(
-            *[controller.states["F_" + muscle_name_list[x]].cx for x in range(len(muscle_name_list))]
+            *[
+                controller.states["F_" + muscle_name_list[x]].cx / muscle_model[x].fmax
+                for x in range(len(muscle_name_list))
+            ]
         )
         return muscle_force
 
@@ -69,6 +77,7 @@ def minimize_overall_stimulation_charge(controller: PenaltyController) -> MX:
                 stim_charge = vertcat(
                     *[
                         controller.controls["last_pulse_width_" + muscle_name_list[x]].cx
+                        / controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0]
                         for x in range(len(muscle_name_list))
                     ]
                 )

cocofest/custom_objectives.py

@@ -94,7 +94,9 @@ def inverse_kinematics_cycling(
     model = biorbd.Model(model_path)
 
     z = model.markers(np.array([0] * model.nbQ()))[0].to_array()[2]
-    if z != model.markers(np.array([np.pi / 2] * model.nbQ()))[0].to_array()[2]:
+    if not np.isclose(
+        z, model.markers(np.array([np.pi / 2] * model.nbQ()))[0].to_array()[2], rtol=1e-05, atol=1e-08, equal_nan=False
+    ):
         print("The model not strictly 2d. Warm start not optimal.")
 
     f = interp1d(

cocofest/dynamics/inverse_kinematics_and_dynamics.py

@@ -43,7 +43,7 @@ def __init__(
         )
         self._with_fatigue = True
         self.stim_time = stim_time
-        self.fmax = 315
+        self.fmax = 248
 
         # --- Default values --- #
         ALPHA_A_DEFAULT = -4.0 * 10e-2  # Value from Ding's experimentation [1] (s^-2)

cocofest/models/ding2007/ding2007_with_fatigue.py

@@ -12,6 +12,7 @@
     DynamicsEvaluation,
     ParameterList,
     ContactType,
+    OdeSolver,
 )
 
 from ..models.fes_model import FesModel
@@ -199,9 +200,16 @@ def muscle_dynamic(
         ddq = nlp.model.forward_dynamics(with_contact=with_contact)(
             q, qdot, total_torque, external_forces, parameters
         )  # q, qdot, tau, external_forces, parameters
+
         dxdt = vertcat(dxdt_muscle_list, dq, ddq)
 
-        return DynamicsEvaluation(dxdt=dxdt, defects=None)
+        defects = None
+        if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION):
+            slope_q = DynamicsFunctions.get(nlp.states_dot["q"], nlp.states_dot.scaled.cx)
+            slope_qdot = DynamicsFunctions.get(nlp.states_dot["qdot"], nlp.states_dot.scaled.cx)
+            defects = vertcat(slope_q, slope_qdot) * nlp.dt - vertcat(dq, ddq) * nlp.dt
+
+        return DynamicsEvaluation(dxdt=dxdt, defects=defects)
 
     @staticmethod
     def muscles_joint_torque(

cocofest/models/dynamical_model.py

@@ -8,6 +8,7 @@
 class FesModel(ABC):
     def __init__(self):
         self.stim_time = None
+        self.previous_stim = None
 
     @abstractmethod
     def set_a_rest(self, model, a_rest: MX | float):

cocofest/models/fes_model.py

@@ -4,10 +4,8 @@
 from bioptim import (
     MultiCyclicNonlinearModelPredictiveControl,
     MultiCyclicCycleSolutions,
-    OdeSolver,
     OptimalControlProgram,
     InitialGuessList,
-    InterpolationType,
     ParameterList,
     VariableScaling,
     Solution,
@@ -37,8 +35,8 @@ def advance_window_initial_guess_states(self, sol, n_cycles_simultaneous=None):
         return True
 
     def advance_window_bounds_controls(self, sol, n_cycles_simultaneous=None, **extra):
-        super(FesNmpc, self).advance_window_bounds_controls(sol)
-        return True
+        bound_have_changed = super(FesNmpc, self).advance_window_bounds_controls(sol)
+        return bound_have_changed
 
     @staticmethod
     def build_new_model(model, previous_stim):

cocofest/optimization/fes_nmpc.py

@@ -1,4 +1,5 @@
 import numpy as np
+from copy import deepcopy
 from casadi import SX
 from bioptim import (
     BiorbdModel,
@@ -11,7 +12,7 @@
     Solver,
     MultiCyclicCycleSolutions,
     ExternalForceSetTimeSeries,
-    OdeSolver,
+    ControlType,
 )
 from .fes_nmpc import FesNmpc
 from ..models.dynamical_model import FesMskModel
@@ -99,7 +100,9 @@ def _initialize_solution(self, dt: float, states: list, controls: list, paramete
             dynamics=self.nlp[0].dynamics_type,
             n_shooting=self.total_optimization_run * self.cycle_len,
             phase_time=self.total_optimization_run * self.cycle_len * dt,
+            x_bounds=self.nlp[0].x_bounds,
             x_init=x_init,
+            u_bounds=self.nlp[0].u_bounds,
             u_init=u_init,
             use_sx=self.cx == SX,
             parameters=parameters,
@@ -108,6 +111,65 @@ def _initialize_solution(self, dt: float, states: list, controls: list, paramete
         a_init = InitialGuessList()
         return Solution.from_initial_guess(solution_ocp, [np.array([dt]), x_init, u_init, p_init, a_init])
 
+    def _initialize_one_cycle(self, dt: float, states: np.ndarray, controls: np.ndarray, parameters: np.ndarray):
+        """return a solution for a single window kept of the MHE"""
+        x_init = InitialGuessList()
+        for key in self.nlp[0].states.keys():
+            x_init.add(
+                key,
+                states[key],
+                interpolation=self.nlp[0].x_init.type,
+                phase=0,
+            )
+
+        u_init = InitialGuessList()
+        u_init_for_solution = InitialGuessList()
+        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.nlp[0].control_type == ControlType.CONSTANT:
+                controls_tp = controls_tp[:, :-1]
+            u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME, phase=0)
+
+        model_serialized = self.nlp[0].model.serialize()
+        model_class = model_serialized[0]
+        model_initializer = model_serialized[1]
+
+        param_list = ParameterList(use_sx=self.cx == SX)
+        p_init = InitialGuessList()
+        for key in self.nlp[0].parameters.keys():
+            parameters_tp = parameters[key]
+            param_list.add(
+                name=key,
+                function=self.nlp[0].parameters[key].function,
+                size=self.nlp[0].parameters[key].shape,
+                scaling=self.nlp[0].parameters[key].scaling,
+            )
+            p_init.add(
+                key,
+                parameters_tp,
+                interpolation=InterpolationType.EACH_FRAME,
+                phase=0,
+            )
+
+        solution_ocp = OptimalControlProgram(
+            bio_model=model_class(**model_initializer),
+            dynamics=self.nlp[0].dynamics_type,
+            objective_functions=deepcopy(self.common_objective_functions),
+            n_shooting=self.cycle_len,
+            phase_time=self.cycle_len * dt,
+            x_bounds=self.nlp[0].x_bounds,
+            x_init=x_init,
+            u_bounds=self.nlp[0].u_bounds,
+            u_init=u_init,
+            use_sx=self.cx == SX,
+            parameters=param_list,
+            parameter_init=p_init,
+            parameter_bounds=self.parameter_bounds,
+        )
+        a_init = InitialGuessList()
+        return Solution.from_initial_guess(solution_ocp, [np.array([dt]), x_init, u_init_for_solution, p_init, a_init])
+
     def create_model_from_list(self, models: list):
         if isinstance(models[0], BiorbdModel):
             return models[0]

cocofest/optimization/fes_nmpc_multibody.py

@@ -127,19 +127,22 @@ def set_x_bounds(model):
                 interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
             )
 
+        return x_bounds
+
+    @staticmethod
+    def set_x_init(model):
+        variable_bound_list = model.name_dof
         x_init = InitialGuessList()
         for j in range(len(variable_bound_list)):
             x_init.add(variable_bound_list[j], model.standard_rest_values()[j])
 
-        return x_bounds, x_init
+        return x_init
 
     @staticmethod
     def set_u_bounds(model, max_bound: int | float):
         u_bounds = BoundsList()  # Controls bounds
-        u_init = InitialGuessList()  # Controls initial guess
 
         if isinstance(model, DingModelPulseWidthFrequency):
-            u_init.add(key="last_pulse_width", initial_guess=[0], phase=0)
             min_pulse_width = model.pd0 if isinstance(model.pd0, int | float) else 0
             u_bounds.add(
                 "last_pulse_width",
@@ -149,7 +152,6 @@ def set_u_bounds(model, max_bound: int | float):
             )
 
         if isinstance(model, DingModelPulseIntensityFrequency):
-            u_init.add(key="pulse_intensity", initial_guess=[0] * model.sum_stim_truncation, phase=0)
             min_pulse_intensity = (
                 model.min_pulse_intensity() if isinstance(model.min_pulse_intensity(), int | float) else 0
             )
@@ -160,7 +162,19 @@ def set_u_bounds(model, max_bound: int | float):
                 interpolation=InterpolationType.CONSTANT,
             )
 
-        return u_bounds, u_init
+        return u_bounds
+
+    @staticmethod
+    def set_u_init(model):
+        u_init = InitialGuessList()  # Controls initial guess
+
+        if isinstance(model, DingModelPulseWidthFrequency):
+            u_init.add(key="last_pulse_width", initial_guess=[0], phase=0)
+
+        if isinstance(model, DingModelPulseIntensityFrequency):
+            u_init.add(key="pulse_intensity", initial_guess=[0] * model.sum_stim_truncation, phase=0)
+
+        return u_init
 
     # TODO: Remove this method
     @staticmethod