Merge pull request #7 from Kev1CO/update_bio

Modification for bioptim update

cocofest/examples/getting_started/pulse_duration_optimization_musculoskeletal_dynamic.py

@@ -36,6 +36,6 @@
 )
 
 if __name__ == "__main__":
-    sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
+    sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=2000))
     sol.animate()
     sol.graphs(show_bounds=False)

cocofest/models/dynamical_model.py

@@ -1,14 +1,15 @@
 from typing import Callable
 import numpy as np
 
-from casadi import vertcat, MX, SX
+from casadi import vertcat, MX, SX, Function
 from bioptim import (
     BiorbdModel,
     OptimalControlProgram,
     NonLinearProgram,
     ConfigureProblem,
     DynamicsFunctions,
     DynamicsEvaluation,
+    ParameterList,
 )
 
 from ..models.fes_model import FesModel
@@ -29,6 +30,7 @@ def __init__(
         activate_force_length_relationship: bool = False,
         activate_force_velocity_relationship: bool = False,
         activate_residual_torque: bool = False,
+        parameters: ParameterList = None,
     ):
         """
         The custom model that will be used in the optimal control program for the FES-MSK models
@@ -47,11 +49,13 @@ def __init__(
             If the force-velocity relationship should be activated
         activate_residual_torque: bool
             If the residual torque should be activated
+        parameters: ParameterList
+            The parameters that will be used in the model
         """
-
-        super().__init__(biorbd_path)
+        super().__init__(biorbd_path, parameters=parameters)
         self._name = name
-        self.bio_model = BiorbdModel(biorbd_path)
+        self.bio_model = BiorbdModel(biorbd_path, parameters=parameters)
+        self.biorbd_path = biorbd_path
 
         self._model_sanity(
             muscles_model,
@@ -159,8 +163,7 @@ def muscle_dynamic(
         # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq)
         dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
         total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau
-        ddq = nlp.model.forward_dynamics(q, qdot, total_torque)
-
+        ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, total_torque, [], parameters)
         dxdt = vertcat(dxdt_muscle_list, dq, ddq)
 
         return DynamicsEvaluation(dxdt=dxdt, defects=None)
@@ -184,9 +187,15 @@ def muscles_joint_torque(
         muscle_forces = vertcat()
         muscle_idx_list = []
 
-        updatedModel = nlp.model.bio_model.model.UpdateKinematicsCustom(q, qdot)
-        nlp.model.bio_model.model.updateMuscles(updatedModel, q, qdot)
-        updated_muscle_length_jacobian = nlp.model.bio_model.model.musclesLengthJacobian(updatedModel, q, False).to_mx()
+        Q = nlp.model.bio_model.q
+        Qdot = nlp.model.bio_model.qdot
+
+        updatedModel = nlp.model.bio_model.model.UpdateKinematicsCustom(Q, Qdot)
+        nlp.model.bio_model.model.updateMuscles(updatedModel, Q, Qdot)
+        updated_muscle_length_jacobian = nlp.model.bio_model.model.musclesLengthJacobian(updatedModel, Q, False).to_mx()
+        updated_muscle_length_jacobian = Function("musclesLengthJacobian", [Q, Qdot], [updated_muscle_length_jacobian])(
+            q, qdot
+        )
 
         bio_muscle_names_at_index = []
         for i in range(len(nlp.model.bio_model.model.muscles())):
@@ -205,22 +214,28 @@ def muscles_joint_torque(
                 muscle_force_length_coefficient(
                     model=updatedModel,
                     muscle=nlp.model.bio_model.model.muscle(muscle_idx),
-                    q=q,
+                    q=Q,
                 )
                 if nlp.model.activate_force_velocity_relationship
                 else 1
             )
+            muscle_force_length_coeff = Function("muscle_force_length_coeff", [Q, Qdot], [muscle_force_length_coeff])(
+                q, qdot
+            )
 
             muscle_force_velocity_coeff = (
                 muscle_force_velocity_coefficient(
                     model=updatedModel,
                     muscle=nlp.model.bio_model.model.muscle(muscle_idx),
-                    q=q,
-                    qdot=qdot,
+                    q=Q,
+                    qdot=Qdot,
                 )
                 if nlp.model.activate_force_velocity_relationship
                 else 1
             )
+            muscle_force_velocity_coeff = Function(
+                "muscle_force_velocity_coeff", [Q, Qdot], [muscle_force_velocity_coeff]
+            )(q, qdot)
 
             muscle_dxdt = muscle_model.dynamics(
                 time,

cocofest/optimization/fes_identification_ocp.py

@@ -114,6 +114,9 @@ def prepare_ocp(
             pulse_intensity=pulse_intensity,
             use_sx=use_sx,
         )
+
+        OcpFesId.update_model_param(model, parameters)
+
         dynamics = OcpFesId._declare_dynamics(model=model)
         x_bounds, x_init = OcpFesId._set_bounds(
             model=model,

cocofest/optimization/fes_ocp.py

@@ -67,6 +67,7 @@ def _prepare_optimization_problem(input_dict: dict) -> dict:
             pulse_intensity=pulse_intensity,
             use_sx=input_dict["use_sx"],
         )
+        OcpFes.update_model_param(input_dict["model"], parameters)
 
         dynamics = OcpFes._declare_dynamics(input_dict["model"])
         x_bounds, x_init = OcpFes._set_bounds(input_dict["model"])
@@ -772,3 +773,11 @@ def _set_objective(n_shooting, objective):
         #     objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=0.001, quadratic=True)
 
         return objective_functions
+
+    @staticmethod
+    def update_model_param(model, parameters):
+        for param_key in parameters:
+            if parameters[param_key].function:
+                param_scaling = parameters[param_key].scaling.scaling
+                param_reduced = parameters[param_key].cx
+                parameters[param_key].function(model, param_reduced * param_scaling, **parameters[param_key].kwargs)

cocofest/optimization/fes_ocp_dynamics.py

@@ -125,8 +125,19 @@ def _prepare_optimization_problem(input_dict: dict) -> dict:
             input_dict["n_cycles_simultaneous"] if "n_cycles_simultaneous" in input_dict.keys() else 1,
         )
 
+        # rebuilding model for the OCP
+        model = FesMskModel(
+            name=input_dict["model"].name,
+            biorbd_path=input_dict["model"].biorbd_path,
+            muscles_model=input_dict["model"].muscles_dynamics_model,
+            activate_force_length_relationship=input_dict["model"].activate_force_length_relationship,
+            activate_force_velocity_relationship=input_dict["model"].activate_force_velocity_relationship,
+            activate_residual_torque=input_dict["model"].activate_residual_torque,
+            parameters=parameters,
+        )
+
         optimization_dict = {
-            "model": input_dict["model"],
+            "model": model,
             "dynamics": dynamics,
             "n_shooting": input_dict["n_shooting"],
             "final_time": input_dict["final_time"],