From d366928512f1ddfbe4b56654911dbffd8d5b37c2 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Wed, 19 Nov 2025 19:26:16 -0500 Subject: [PATCH 1/5] Updating to new bioptim version --- cocofest/custom_objectives.py | 6 +- cocofest/integration/ivp_fes.py | 23 +-- cocofest/models/ding2003/ding2003.py | 144 +++++++--------- .../models/ding2003/ding2003_with_fatigue.py | 141 +++------------- cocofest/models/ding2007/ding2007.py | 143 +++------------- .../models/ding2007/ding2007_with_fatigue.py | 156 +++-------------- cocofest/models/dynamical_model.py | 159 ++++++++---------- cocofest/models/fes_model.py | 38 +---- cocofest/models/hmed2018/hmed2018.py | 153 ++++------------- .../models/hmed2018/hmed2018_with_fatigue.py | 154 +++-------------- cocofest/models/marion2009/marion2009.py | 98 ++--------- .../models/marion2009/marion2009_modified.py | 102 +++-------- .../marion2009_modified_with_fatigue.py | 108 +++--------- .../marion2009/marion2009_with_fatigue.py | 103 +++--------- cocofest/models/marion2013/marion2013.py | 98 +++-------- .../models/marion2013/marion2013_modified.py | 99 +++-------- .../marion2013_modified_with_fatigue.py | 118 +++---------- .../marion2013/marion2013_with_fatigue.py | 113 +++---------- cocofest/models/state_configure.py | 64 ++++--- cocofest/models/veltink1992/veltink1992.py | 102 +++-------- .../veltink1992/veltink1992_and_riener1998.py | 100 ++--------- cocofest/optimization/fes_ocp.py | 25 +-- cocofest/optimization/fes_ocp_multibody.py | 19 +-- .../cycling/cycling_bayesian_mhe.py | 6 +- .../cycling/cycling_pulse_width_mhe.py | 24 +-- .../cycling_with_different_driven_methods.py | 2 - .../elbow_flexion/elbow_flexion_task.py | 12 +- .../frequency_optimization_multibody.py | 12 +- .../fes_multibody/reaching/reaching_task.py | 20 +-- .../identification/muscle_model_id.py | 10 +- .../pulse_intensity_optimization_multibody.py | 6 +- .../pulse_width_optimization_multibody.py | 7 +- .../optimization/force_tracking.py | 13 +- .../optimization/frequency_optimization.py | 16 +- .../pulse_intensity_optimization.py | 7 +- .../optimization/pulse_width_optimization.py | 7 +- .../pulse_width_optimization_nmpc.py | 9 +- .../force_model/ding2003_model_id.py | 10 +- .../force_model/ding2007_model_id.py | 13 +- .../force_model/hmed2018_model_id.py | 15 +- .../other_fes_models/marion2009_example.py | 6 +- .../other_fes_models/marion2013_example.py | 6 +- .../other_fes_models/veltink1992_example.py | 16 +- ...force_velocity_relationships_comparison.py | 65 ------- .../muscle_relationships_comparison.py | 128 ++++++++++++++ external/bioptim | 2 +- .../test_models_dynamics_without_bioptim.py | 96 ++++------- tests/shard1/test_ocp_id.py | 21 +-- tests/shard2/test_run_examples.py | 18 +- 49 files changed, 787 insertions(+), 2026 deletions(-) delete mode 100644 examples/sensitivity/force_length_velocity/muscle_force_length_and_force_velocity_relationships_comparison.py create mode 100644 examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py diff --git a/cocofest/custom_objectives.py b/cocofest/custom_objectives.py index af4acd68..4360def6 100644 --- a/cocofest/custom_objectives.py +++ b/cocofest/custom_objectives.py @@ -23,7 +23,7 @@ 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_name_list = controller.model.muscle_names muscle_model = controller.model.muscles_dynamics_model muscle_fatigue = vertcat( *[ @@ -47,7 +47,7 @@ def minimize_overall_muscle_force_production(controller: PenaltyController) -> M ------- The sum of each force """ - muscle_name_list = controller.model.bio_model.muscle_names + muscle_name_list = controller.model.muscle_names muscle_model = controller.model.muscles_dynamics_model muscle_force = vertcat( *[ @@ -72,7 +72,7 @@ def minimize_overall_stimulation_charge(controller: PenaltyController) -> MX: The sum of each stimulation control """ if isinstance(controller.model, FesMskModel): - muscle_name_list = controller.model.bio_model.muscle_names + muscle_name_list = controller.model.muscle_names if isinstance(controller.model.muscles_dynamics_model[0], DingModelPulseWidthFrequency): stim_charge = vertcat( *[ diff --git a/cocofest/integration/ivp_fes.py b/cocofest/integration/ivp_fes.py index 53a5fc7b..b081a81f 100644 --- a/cocofest/integration/ivp_fes.py +++ b/cocofest/integration/ivp_fes.py @@ -1,12 +1,10 @@ import numpy as np from bioptim import ( ControlType, - DynamicsList, InitialGuessList, OdeSolver, OptimalControlProgram, ParameterList, - PhaseDynamics, BoundsList, InterpolationType, Solution, @@ -20,6 +18,7 @@ from cocofest.models.ding2007.ding2007_with_fatigue import DingModelPulseWidthFrequencyWithFatigue from cocofest.models.hmed2018.hmed2018 import DingModelPulseIntensityFrequency from cocofest.models.hmed2018.hmed2018_with_fatigue import DingModelPulseIntensityFrequencyWithFatigue +from cocofest.optimization.fes_ocp import OcpFes class IvpFes: @@ -90,9 +89,9 @@ def __init__( numerical_data_time_series, stim_idx_at_node_list = self.model.get_numerical_data_time_series( self.n_shooting, self.final_time ) - self.ode_solver = self.ivp_parameters["ode_solver"] - self._declare_dynamics(numerical_data_time_series) + self.dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=self.ode_solver) ( self.x_init, @@ -285,7 +284,7 @@ def _prepare_fake_ocp(self): return OptimalControlProgram( bio_model=[self.model], - dynamics=self.dynamics, + dynamics=self.dynamics_options, n_shooting=self.n_shooting, phase_time=self.final_time, x_init=self.x_init, @@ -318,20 +317,6 @@ def integrate( duplicated_times=duplicated_times, ) - def _declare_dynamics(self, numerical_data_time_series=None): - - self.dynamics = DynamicsList() - self.dynamics.add( - self.model.declare_ding_variables, - dynamic_function=self.model.dynamics, - expand_dynamics=True, - expand_continuity=False, - phase=0, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_data_time_series, - ode_solver=self.ode_solver, - ) - def build_initial_guess_from_ocp(self, ocp, stim_idx_at_node_list=None): """ Build a state, control, parameters and stochastic initial guesses for each phases from a given ocp diff --git a/cocofest/models/ding2003/ding2003.py b/cocofest/models/ding2003/ding2003.py index 2b07193d..36d43f3d 100644 --- a/cocofest/models/ding2003/ding2003.py +++ b/cocofest/models/ding2003/ding2003.py @@ -6,17 +6,17 @@ from casadi import MX, exp, vertcat from bioptim import ( - ConfigureProblem, DynamicsEvaluation, NonLinearProgram, - OptimalControlProgram, + StateDynamics, + FcnEnum, ) from cocofest.models.state_configure import StateConfigure from cocofest.models.fes_model import FesModel -class DingModelFrequency(FesModel): +class DingModelFrequency(StateDynamics, FesModel): """ This is a custom model of the Bioptim package. As CustomModel, some methods are mandatory and must be implemented. to make it work with bioptim. @@ -68,6 +68,16 @@ def __init__( self.km_rest = KM_REST_DEFAULT self.fmax = 315.5 # Maximum force (N) at 100 Hz + # ---- Muscle relationship ---- # + self.fes_model = None + self.force_length_relationship = 1 + self.force_velocity_relationship = 1 + self.passive_force_relationship = 0 + + # ---- Declare dynamic variable ---- # + self.state_configuration = [CustomStates.my_muscle_states] + self.contact_types = () + def set_a_rest(self, model, a_rest: MX | float): # models is required for bioptim compatibility self.a_rest = a_rest @@ -155,38 +165,34 @@ def get_lambda_i(nb_stim: int, pulse_intensity: MX | float) -> list[MX | float]: # ---- Model's dynamics ---- # def system_dynamics( self, - cn: MX, - f: MX, - t: MX = None, - t_stim_prev: list[MX] = None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics is the function that describes the models. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[MX] - The time list of the previous stimulations (s) - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) + time: MX + The system's current node time + states: MX + The state of the system CN, F + controls: MX + The controls of the system, none + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) f_dot = self.f_dot_fun( cn, @@ -194,9 +200,6 @@ def system_dynamics( self.a_rest, self.tau1_rest, self.km_rest, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, ) # Equation n°2 return vertcat(cn_dot, f_dot) @@ -285,9 +288,6 @@ def f_dot_fun( a: MX | float, tau1: MX | float, km: MX | float, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, ) -> MX | float: """ Parameters @@ -302,23 +302,16 @@ def f_dot_fun( The previous step value of time_state_force_no_cross_bridge (s) km: MX | float The previous step value of cross_bridges (unitless) - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) - Returns ------- The value of the derivative force (N) """ return (a * (cn / (km + cn)) - (f / (tau1 + self.tau2 * (cn / (km + cn))))) * ( - force_length_relationship * force_velocity_relationship + passive_force_relationship + self.force_length_relationship * self.force_velocity_relationship + self.passive_force_relationship ) # Equation n°2 - @staticmethod def dynamics( + self, time: MX, states: MX, controls: MX, @@ -326,10 +319,6 @@ def dynamics( algebraic_states: MX, numerical_timeseries: MX, nlp: NonLinearProgram, - fes_model=None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, ) -> DynamicsEvaluation: """ Functional electrical stimulation dynamic @@ -350,57 +339,41 @@ def dynamics( The numerical timeseries of the system nlp: NonLinearProgram A reference to the phase - fes_model: DingModelFrequency - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) Returns ------- The derivative of the states in the tuple[MX] format """ - model = fes_model if fes_model else nlp.model + model = self.fes_model if self.fes_model else nlp.model dxdt_fun = model.system_dynamics + defects = None + # TODO: Get defects for fes model states + # 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 + + # if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): + # slope_states = vertcat(*[DynamicsFunctions.get(nlp.states_dot[key], nlp.states_dot.scaled.cx) for key in nlp.states.keys()]) + # dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) + # total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau + # external_forces = nlp.get_external_forces("external_forces", states, controls, algebraic_states, + # numerical_data_timeseries) + # ddq = nlp.model.forward_dynamics(with_contact=self.with_contact)( + # q, qdot, total_torque, external_forces, parameters + # ) # q, qdot, tau, external_forces, parameters + # defects = slope_states * nlp.dt - vertcat(dq, ddq) * nlp.dt + return DynamicsEvaluation( dxdt=dxdt_fun( - cn=states[0], - f=states[1], - t=time, - t_stim_prev=numerical_timeseries, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, + time=time, + states=states, + controls=controls, + numerical_timeseries=numerical_timeseries, ), + defects=defects, ) - def declare_ding_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: list = (), - ): - """ - Tell the program which variables are states and controls. - The user is expected to use the ConfigureProblem.configure_xxx functions. - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node. Experimental external forces should be included here. - contact_type: list - A list of contact types. This is used to define the contact forces in the dynamics. Not used in this model. - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) - def _get_additional_previous_stim_time(self): while len(self.previous_stim["time"]) < self.sum_stim_truncation: self.previous_stim["time"].insert(0, -10000000) @@ -465,3 +438,8 @@ def get_n_shooting(self, final_time: float) -> int: ) return n_shooting + + +# TODO: Change to future bioptim version +class CustomStates(FcnEnum): + my_muscle_states = (StateConfigure().configure_all_muscle_states,) diff --git a/cocofest/models/ding2003/ding2003_with_fatigue.py b/cocofest/models/ding2003/ding2003_with_fatigue.py index 13dd4038..c417b1e1 100644 --- a/cocofest/models/ding2003/ding2003_with_fatigue.py +++ b/cocofest/models/ding2003/ding2003_with_fatigue.py @@ -139,47 +139,37 @@ def identifiable_parameters(self): # ---- Model's dynamics ---- # def system_dynamics( self, - cn: MX, - f: MX, - a: MX = None, - tau1: MX = None, - km: MX = None, - t: MX = None, - t_stim_prev: MX | float = None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics is the function that describes the models. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - a: MX - The value of the scaling factor (unitless) - tau1: MX - The value of the time_state_force_no_cross_bridge (s) - km: MX - The value of the cross_bridges (unitless) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: MX | float - The previous time at which the stimulation was applied (s) - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) + time: MX + The system's current node time + states: MX + The state of the system CN, F, A, Tau1, Km + controls: MX + The controls of the system, none + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + a = states[2] + tau1 = states[3] + km = states[4] + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) # Equation n°1 f_dot = self.f_dot_fun( cn, @@ -187,9 +177,6 @@ def system_dynamics( a, tau1, km, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, ) # Equation n°2 a_dot = self.a_dot_fun(a, f) # Equation n°5 tau1_dot = self.tau1_dot_fun(tau1, f) # Equation n°9 @@ -240,91 +227,3 @@ def km_dot_fun(self, km: MX, f: MX) -> MX | float: The value of the derivative cross_bridges (unitless) """ return -(km - self.km_rest) / self.tau_fat + self.alpha_km * f # Equation n°11 - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system, none - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: DingModelFrequencyWithFatigue - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - a=states[2], - tau1=states[3], - km=states[4], - t=time, - t_stim_prev=numerical_timeseries, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, - ), - defects=None, - ) - - def declare_ding_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: list = (), - ): - """ - Tell the program which variables are states and controls. - The user is expected to use the ConfigureProblem.configure_xxx functions. - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node. Experimental external forces should be included here. - contact_type: list - A list of contact types. This is used to define the contact forces in the dynamics. Not used in this model. - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) diff --git a/cocofest/models/ding2007/ding2007.py b/cocofest/models/ding2007/ding2007.py index 3f2223a1..a7491f97 100644 --- a/cocofest/models/ding2007/ding2007.py +++ b/cocofest/models/ding2007/ding2007.py @@ -1,14 +1,9 @@ from typing import Callable -import numpy as np from casadi import MX, vertcat, exp -from bioptim import ( - ConfigureProblem, - DynamicsEvaluation, - NonLinearProgram, - OptimalControlProgram, -) +from bioptim import FcnEnum + from cocofest.models.ding2003.ding2003 import DingModelFrequency from cocofest.models.state_configure import StateConfigure @@ -66,6 +61,8 @@ def __init__( self.tauc = TAUC_DEFAULT self.fmax = 248 # Maximum force (N) at 100 Hz and 600 us + self.control_configuration = [CustomStates.my_control] + @property def identifiable_parameters(self): return { @@ -108,41 +105,35 @@ def serialize(self) -> tuple[Callable, dict]: def system_dynamics( self, - cn: MX, - f: MX, - t: MX = None, - t_stim_prev: list[float] | list[MX] = None, - pulse_width: MX = None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics is the function that describes the models. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[float] | list[MX] - The time list of the previous stimulations (s) - pulse_width: MX - The pulsation duration of the current stimulation (s) - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force coefficient of the muscle (unitless) + time: MX + The system's current node time + states: MX + The state of the system CN, F + controls: MX + The controls of the system, pulse_width + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + pulse_width = controls[0] + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) a_scale = self.a_calculation(a_scale=self.a_scale, pulse_width=pulse_width) f_dot = self.f_dot_fun( @@ -151,9 +142,6 @@ def system_dynamics( a_scale, self.tau1_rest, self.km_rest, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, ) # Equation n°2 from Ding's 2003 article return vertcat(cn_dot, f_dot) @@ -211,89 +199,8 @@ def set_impulse_width(self, value: list[MX]): """ self.pulse_width = value - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system, none - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: DingModelPulseWidthFrequency - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force coefficient of the muscle (unitless) - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - t=time, - t_stim_prev=numerical_timeseries, - pulse_width=controls[0], - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, - ), - defects=None, - ) +# TODO: Change to future bioptim version +class CustomStates(FcnEnum): + my_control = (StateConfigure().configure_last_pulse_width,) - def declare_ding_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: list = (), - ): - """ - Tell the program which variables are states and controls. - The user is expected to use the ConfigureProblem.configure_xxx functions. - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node. Experimental external forces should be included here. - contact_type: list - A list of contact types. This is used to define the contact forces in the dynamics. Not used in this model. - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - StateConfigure().configure_last_pulse_width(ocp, nlp) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) diff --git a/cocofest/models/ding2007/ding2007_with_fatigue.py b/cocofest/models/ding2007/ding2007_with_fatigue.py index c930f8aa..13e75829 100644 --- a/cocofest/models/ding2007/ding2007_with_fatigue.py +++ b/cocofest/models/ding2007/ding2007_with_fatigue.py @@ -3,14 +3,7 @@ from casadi import MX, vertcat import numpy as np -from bioptim import ( - ConfigureProblem, - DynamicsEvaluation, - NonLinearProgram, - OptimalControlProgram, -) from cocofest.models.ding2007.ding2007 import DingModelPulseWidthFrequency -from cocofest.models.state_configure import StateConfigure class DingModelPulseWidthFrequencyWithFatigue(DingModelPulseWidthFrequency): @@ -124,50 +117,38 @@ def serialize(self) -> tuple[Callable, dict]: def system_dynamics( self, - cn: MX, - f: MX, - a: MX = None, - tau1: MX = None, - km: MX = None, - t: MX = None, - t_stim_prev: MX = None, - pulse_width: MX = None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics is the function that describes the models. Parameters - ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - a: MX - The value of the scaling factor (unitless) - tau1: MX - The value of the time_state_force_no_cross_bridge (ms) - km: MX - The value of the cross_bridges (unitless) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: MX - The time of the previous stimulation (s) - pulse_width: MX - The time of the impulse (s) - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) + # ---------- + time: MX + The system's current node time + states: MX + The state of the system CN, F, A, Tau1, Km + controls: MX + The controls of the system, pulse_width + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + a = states[2] + tau1 = states[3] + km = states[4] + pulse_width = controls[0] + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) a_scale = self.a_calculation(a_scale=a, pulse_width=pulse_width) @@ -177,9 +158,6 @@ def system_dynamics( a_scale, tau1, km, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, ) # Equation n°2 from Ding's 2003 article a_dot = self.a_dot_fun(a, f) @@ -231,93 +209,3 @@ def km_dot_fun(self, km: MX, f: MX) -> MX | float: The value of the derivative cross_bridges (unitless) """ return -(km - self.km_rest) / self.tau_fat + self.alpha_km * f # Equation n°11 - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system, none - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: DingModelPulseWidthFrequencyWithFatigue - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - a=states[2], - tau1=states[3], - km=states[4], - t=time, - t_stim_prev=numerical_timeseries, - pulse_width=controls[0], - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, - ), - defects=None, - ) - - def declare_ding_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: list = (), - ): - """ - Tell the program which variables are states and controls. - The user is expected to use the ConfigureProblem.configure_xxx functions. - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node. Experimental external forces should be included here. - contact_type: list - A list of contact types. This is used to define the contact forces in the dynamics. Not used in this model. - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - StateConfigure().configure_last_pulse_width(ocp, nlp) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index f1ed9f5e..1d6a6c80 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -4,15 +4,17 @@ from casadi import vertcat, MX, SX, Function from bioptim import ( BiorbdModel, + ConfigureVariables, + DynamicsEvaluation, + DynamicsFunctions, ExternalForceSetTimeSeries, - OptimalControlProgram, + FcnEnum, NonLinearProgram, - ConfigureProblem, - DynamicsFunctions, - DynamicsEvaluation, - ParameterList, - ContactType, OdeSolver, + OptimalControlProgram, + ParameterList, + StateDynamics, + States ) from ..models.fes_model import FesModel @@ -26,8 +28,7 @@ muscle_passive_force_coefficient, ) - -class FesMskModel(BiorbdModel): +class FesMskModel(StateDynamics, BiorbdModel): def __init__( self, name: str = None, @@ -41,6 +42,7 @@ def __init__( activate_residual_torque: bool = False, parameters: ParameterList = None, external_force_set: ExternalForceSetTimeSeries = None, + with_contact=False, ): """ The custom model that will be used in the optimal control program for the FES-MSK models @@ -64,8 +66,8 @@ def __init__( parameters: ParameterList The parameters that will be used in the model """ - super().__init__(biorbd_path, parameters=parameters, external_force_set=external_force_set) - self.bio_model = BiorbdModel(biorbd_path, parameters=parameters, external_force_set=external_force_set) + super().__init__(bio_model=biorbd_path, parameters=parameters, external_force_set=external_force_set) + self._name = name self.biorbd_path = biorbd_path @@ -84,14 +86,19 @@ def __init__( ) self.muscles_dynamics_model[i].all_stim = self.muscles_dynamics_model[i].previous_stim["time"] + stim_time - self.bio_stim_model = [self.bio_model] + self.muscles_dynamics_model - + self.bio_stim_model = [self] + self.muscles_dynamics_model self.activate_force_length_relationship = activate_force_length_relationship self.activate_force_velocity_relationship = activate_force_velocity_relationship self.activate_passive_force_relationship = activate_passive_force_relationship self.activate_residual_torque = activate_residual_torque self.parameters_list = parameters self.external_forces_set = external_force_set + self.muscles_model = muscles_model + self.with_contact = with_contact + + self.state_configuration = [CustomStates.my_muscles_states, States.Q, States.QDOT] + self.control_configuration = [CustomStates.my_controls] + # ---- Absolutely needed methods ---- # def serialize(self) -> tuple[Callable, dict]: @@ -99,7 +106,7 @@ def serialize(self) -> tuple[Callable, dict]: FesMskModel, { "name": self._name, - "biorbd_path": self.bio_model.path, + "biorbd_path": self.path, "muscles_model": self.muscles_dynamics_model, "stim_time": self.muscles_dynamics_model[0].stim_time, "previous_stim": self.muscles_dynamics_model[0].previous_stim, @@ -113,10 +120,6 @@ def serialize(self) -> tuple[Callable, dict]: ) # ---- Needed for the example ---- # - @property - def name_dof(self) -> tuple[str]: - return self.bio_model.name_dof - def muscle_name_dof(self, index: int = 0) -> list[str]: return self.muscles_dynamics_model[index].name_dof(with_muscle_name=True) @@ -125,14 +128,14 @@ def nb_state(self) -> int: nb_state = 0 for muscle_model in self.muscles_dynamics_model: nb_state += muscle_model.nb_state - nb_state += self.bio_model.nb_q + nb_state += self.nb_q return nb_state @property def name(self) -> None | str: return self._name - def muscle_dynamic( + def dynamics( self, time: MX | SX, states: MX | SX, @@ -141,8 +144,6 @@ def muscle_dynamic( algebraic_states: MX | SX, numerical_data_timeseries: MX | SX, nlp: NonLinearProgram, - muscle_models: list[FesModel], - state_name_list=None, ) -> DynamicsEvaluation: """ The custom dynamics function that provides the derivative of the states: dxdt = f(t, x, u, p, s) @@ -163,15 +164,10 @@ def muscle_dynamic( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. nlp: NonLinearProgram A reference to the phase - muscle_models: list[FesModel] - The list of the muscle models - state_name_list: list[str] - The states names list Returns ------- The derivative of the states in the tuple[MX | SX] format """ - q = DynamicsFunctions.get(nlp.states["q"], states) qdot = DynamicsFunctions.get(nlp.states["qdot"], states) tau = DynamicsFunctions.get(nlp.controls["tau"], controls) if "tau" in nlp.controls.keys() else 0 @@ -184,8 +180,6 @@ def muscle_dynamic( algebraic_states, numerical_data_timeseries, nlp, - muscle_models, - state_name_list, q, qdot, ) @@ -193,16 +187,19 @@ 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 - external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_data_timeseries) - with_contact = ( - True if nlp.model.bio_model.contact_names != () else False - ) # TODO: Add a better way of with_contact=True - ddq = nlp.model.forward_dynamics(with_contact=with_contact)( + external_forces = nlp.get_external_forces("external_forces", states, controls, algebraic_states, numerical_data_timeseries) + + # with_contact = ( + # True if nlp.model.bio_model.contact_names != () else False + # ) # TODO: Remove once added in supplementary dynamics + + ddq = nlp.model.forward_dynamics(with_contact=self.with_contact)( q, qdot, total_torque, external_forces, parameters ) # q, qdot, tau, external_forces, parameters dxdt = vertcat(dxdt_muscle_list, dq, ddq) + # TODO: Missing defect for FES states defects = None if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): slope_q = DynamicsFunctions.get(nlp.states_dot["q"], nlp.states_dot.scaled.cx) @@ -220,31 +217,29 @@ def muscles_joint_torque( algebraic_states: MX | SX, numerical_data_timeseries: MX | SX, nlp: NonLinearProgram, - muscle_models: list[FesModel], - state_name_list=None, q: MX | SX = None, qdot: MX | SX = None, ): - dxdt_muscle_list = vertcat() muscle_forces = vertcat() muscle_idx_list = [] + state_name_list=nlp.states.keys() - Q = nlp.model.bio_model.q - Qdot = nlp.model.bio_model.qdot + Q = nlp.model.q + Qdot = nlp.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() + updatedModel = nlp.model.model.UpdateKinematicsCustom(Q, Qdot) + nlp.model.model.updateMuscles(updatedModel, Q, Qdot) + updated_muscle_length_jacobian = nlp.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())): - bio_muscle_names_at_index.append(nlp.model.bio_model.model.muscle(i).name().to_string()) + for i in range(len(nlp.model.model.muscles())): + bio_muscle_names_at_index.append(nlp.model.model.muscle(i).name().to_string()) - for muscle_model in muscle_models: + for muscle_model in nlp.model.muscles_dynamics_model: muscle_states_idxs = [ i for i in range(len(state_name_list)) if muscle_model.muscle_name in state_name_list[i] ] @@ -268,10 +263,10 @@ def muscles_joint_torque( muscle_force_length_coeff = ( muscle_force_length_coefficient( model=updatedModel, - muscle=nlp.model.bio_model.model.muscle(muscle_idx), + muscle=nlp.model.model.muscle(muscle_idx), q=Q, ) - if nlp.model.activate_force_velocity_relationship + if nlp.model.activate_force_length_relationship else 1 ) muscle_force_length_coeff = Function("muscle_force_length_coeff", [Q, Qdot], [muscle_force_length_coeff])( @@ -281,7 +276,7 @@ def muscles_joint_torque( muscle_force_velocity_coeff = ( muscle_force_velocity_coefficient( model=updatedModel, - muscle=nlp.model.bio_model.model.muscle(muscle_idx), + muscle=nlp.model.model.muscle(muscle_idx), q=Q, qdot=Qdot, ) @@ -295,7 +290,7 @@ def muscles_joint_torque( muscle_passive_force_coeff = ( muscle_passive_force_coefficient( model=updatedModel, - muscle=nlp.model.bio_model.model.muscle(muscle_idx), + muscle=nlp.model.model.muscle(muscle_idx), q=Q, ) if nlp.model.activate_passive_force_relationship @@ -313,19 +308,14 @@ def muscles_joint_torque( if external_force_in_numerical_data_timeseries else numerical_data_timeseries ) - muscle_dxdt = muscle_model.dynamics( - time, - muscle_states, - muscle_controls, - muscle_parameters, - algebraic_states, - fes_numerical_data_timeseries, - nlp, - fes_model=muscle_model, - force_length_relationship=muscle_force_length_coeff, - force_velocity_relationship=muscle_force_velocity_coeff, - passive_force_relationship=muscle_passive_force_coeff, - ).dxdt + + muscle_model.fes_model = muscle_model + muscle_model.force_length_relationship=muscle_force_length_coeff + muscle_model.force_velocity_relationship=muscle_force_velocity_coeff + muscle_model.passive_force_relationship=muscle_passive_force_coeff + + muscle_dxdt = muscle_model.dynamics(time, muscle_states, muscle_controls, muscle_parameters, + algebraic_states, fes_numerical_data_timeseries, nlp).dxdt dxdt_muscle_list = vertcat(dxdt_muscle_list, muscle_dxdt) muscle_idx_list.append(muscle_idx) @@ -400,8 +390,6 @@ def forces_from_fes_driven( algebraic_states, numerical_timeseries, nlp, - nlp.model.muscles_dynamics_model, - state_name_list, q, qdot, ) @@ -414,12 +402,16 @@ def forces_from_fes_driven( return nlp.model.rigid_contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) - def declare_model_variables( - self, + + @staticmethod + def declare_model_control( ocp: OptimalControlProgram, nlp: NonLinearProgram, numerical_data_timeseries: dict[str, np.ndarray] = None, contact_type: list = (), + as_states: bool=False, + as_controls: bool=True, + as_algebraic_states: bool=False, ): """ Tell the program which variables are states and controls. @@ -434,35 +426,22 @@ def declare_model_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. contact_type: list The type of contact to be used + as_states: bool + If the variables are states + as_controls: bool + If the variables are controls + as_algebraic_states: bool + If the variables are algebraic states """ - - state_name_list = StateConfigure().configure_all_muscle_states(self.muscles_dynamics_model, ocp, nlp) - ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) - state_name_list.append("q") - ConfigureProblem.configure_qdot(ocp, nlp, as_states=True, as_controls=False) - state_name_list.append("qdot") - for muscle_model in self.muscles_dynamics_model: + for muscle_model in nlp.model.muscles_dynamics_model: if isinstance(muscle_model, DingModelPulseWidthFrequency): StateConfigure().configure_last_pulse_width(ocp, nlp, muscle_name=str(muscle_model.muscle_name)) if isinstance(muscle_model, DingModelPulseIntensityFrequency): StateConfigure().configure_pulse_intensity( ocp, nlp, muscle_name=str(muscle_model.muscle_name), truncation=muscle_model.sum_stim_truncation ) - if self.activate_residual_torque: - ConfigureProblem.configure_tau(ocp, nlp, as_states=False, as_controls=True) - - ConfigureProblem.configure_dynamics_function( - ocp, - nlp, - dyn_func=self.muscle_dynamic, - muscle_models=self.muscles_dynamics_model, - state_name_list=state_name_list, - ) - - if ContactType.RIGID_EXPLICIT in contact_type: - ConfigureProblem.configure_rigid_contact_function( - ocp, nlp, self.forces_from_fes_driven, state_name_list=state_name_list - ) + if nlp.model.activate_residual_torque: + ConfigureVariables.configure_tau(ocp, nlp, as_states=False, as_controls=True, as_algebraic_states=False) @staticmethod def _model_sanity( @@ -503,3 +482,9 @@ def _check_numerical_timeseries_format(numerical_timeseries: np.ndarray, n_shoot f"The numerical_data_timeseries should be of format dict[str, np.ndarray] " f"where the list is the number of shooting points of the phase " ) + + +# TODO: Change to future bioptim version +class CustomStates(FcnEnum): + my_muscles_states = (StateConfigure().configure_all_muscle_msk_states,) + my_controls = (FesMskModel.declare_model_control,) diff --git a/cocofest/models/fes_model.py b/cocofest/models/fes_model.py index bc14b5e5..997117e8 100644 --- a/cocofest/models/fes_model.py +++ b/cocofest/models/fes_model.py @@ -112,13 +112,13 @@ def with_fatigue(self): @abstractmethod def system_dynamics( self, - cn: MX, - f: MX, - cn_sum: MX, - force_length_relationship: MX | float, - force_velocity_relationship: MX | float, - passive_force_relationship: MX | float, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ): + + """ Returns @@ -169,9 +169,6 @@ def f_dot_fun( a: MX | float, tau1: MX | float, km: MX | float, - force_length_relationship: MX | float, - force_velocity_relationship: MX | float, - passive_force_relationship: MX | float, ): """ @@ -180,35 +177,16 @@ def f_dot_fun( """ - @staticmethod @abstractmethod def dynamics( + self, time: MX, states: MX, controls: MX, parameters: MX, algebraic_states: MX, - numerical_data_timeseries: MX, - nlp: NonLinearProgram, - fes_model, - force_length_relationship: MX | float, - force_velocity_relationship: MX | float, - passive_force_relationship: MX | float, - ): - """ - - Returns - ------- - - """ - - @abstractmethod - def declare_ding_variables( - self, - ocp: OptimalControlProgram, + numerical_timeseries: MX, nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: tuple = (), ): """ diff --git a/cocofest/models/hmed2018/hmed2018.py b/cocofest/models/hmed2018/hmed2018.py index 82f970ae..a71a71ea 100644 --- a/cocofest/models/hmed2018/hmed2018.py +++ b/cocofest/models/hmed2018/hmed2018.py @@ -3,12 +3,8 @@ from casadi import MX, vertcat, tanh import numpy as np -from bioptim import ( - ConfigureProblem, - DynamicsEvaluation, - NonLinearProgram, - OptimalControlProgram, -) +from bioptim import FcnEnum + from cocofest.models.ding2003.ding2003 import DingModelFrequency from cocofest.models.state_configure import StateConfigure @@ -62,6 +58,8 @@ def __init__( self.cr = CR_DEFAULT self.impulse_intensity = None + self.control_configuration = [CustomStates.my_control] + @property def identifiable_parameters(self): return { @@ -117,41 +115,35 @@ def serialize(self) -> tuple[Callable, dict]: def system_dynamics( self, - cn: MX, - f: MX, - t: MX = None, - t_stim_prev: list[MX] | list[float] = None, - pulse_intensity: list[MX] | list[float] = None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics is the function that describes the models. - - Parameters - ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[MX] | list[float] - The previous stimulation time (s) - pulse_intensity: list[MX] | list[float] - The pulsation intensity of the current stimulation (mA) - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) + # + # Parameters + # ---------- + time: MX + The system's current node time + states: MX + The state of the system CN, F + controls: MX + The controls of the system, pulse_intensity + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + pulse_intensity = controls + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev, pulse_intensity) f_dot = self.f_dot_fun( cn, @@ -159,9 +151,6 @@ def system_dynamics( self.a_rest, self.tau1_rest, self.km_rest, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, ) # Equation n°2 return vertcat(cn_dot, f_dot) @@ -215,93 +204,6 @@ def set_impulse_intensity(self, value: MX): for i in range(value.shape[0]): self.impulse_intensity.append(value[i]) - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model: NonLinearProgram = None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system, none - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: DingModelPulseIntensityFrequency - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - t=time, - t_stim_prev=numerical_timeseries, - pulse_intensity=controls, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, - ), - defects=None, - ) - - def declare_ding_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: list = (), - ): - """ - Tell the program which variables are states and controls. - The user is expected to use the ConfigureProblem.configure_xxx functions. - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node. Experimental external forces should be included here. - contact_type: list - A list of contact types. This is used to define the contact forces in the dynamics. Not used in this model. - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - StateConfigure().configure_pulse_intensity(ocp, nlp, truncation=self.sum_stim_truncation) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) - def min_pulse_intensity(self): """ Returns @@ -316,3 +218,8 @@ def _get_additional_previous_stim_time(self): self.previous_stim["time"].insert(0, -10000000) self.previous_stim["pulse_intensity"].insert(0, 50) return self.previous_stim + + +# TODO: Change to future bioptim version +class CustomStates(FcnEnum): + my_control = (StateConfigure().configure_pulse_intensity,) diff --git a/cocofest/models/hmed2018/hmed2018_with_fatigue.py b/cocofest/models/hmed2018/hmed2018_with_fatigue.py index f26b8379..41bf4a05 100644 --- a/cocofest/models/hmed2018/hmed2018_with_fatigue.py +++ b/cocofest/models/hmed2018/hmed2018_with_fatigue.py @@ -3,14 +3,7 @@ from casadi import MX, vertcat import numpy as np -from bioptim import ( - ConfigureProblem, - DynamicsEvaluation, - NonLinearProgram, - OptimalControlProgram, -) from cocofest.models.hmed2018.hmed2018 import DingModelPulseIntensityFrequency -from cocofest.models.state_configure import StateConfigure class DingModelPulseIntensityFrequencyWithFatigue(DingModelPulseIntensityFrequency): @@ -123,50 +116,38 @@ def serialize(self) -> tuple[Callable, dict]: def system_dynamics( self, - cn: MX, - f: MX, - a: MX = None, - tau1: MX = None, - km: MX = None, - t: MX = None, - t_stim_prev: list[MX] = None, - pulse_intensity: list[MX] | list[float] = None, - force_length_relationship: float | MX = 1, - force_velocity_relationship: float | MX = 1, - passive_force_relationship: MX | float = 0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics is the function that describes the models. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - a: MX - The value of the scaling factor (unitless) - tau1: MX - The value of the time_state_force_no_cross_bridge (ms) - km: MX - The value of the cross_bridges (unitless) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[MX] - The previous time at which the stimulation was applied (s) - pulse_intensity: list[MX] | list[float] - The intensity of the stimulations (mA) - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) + time: MX + The system's current node time + states: MX + The state of the system CN, F, A, Tau1, Km + controls: MX + The controls of the system, pulse_intensity + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + a = states[2] + tau1 = states[3] + km = states[4] + pulse_intensity = controls # Check + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev, pulse_intensity) f_dot = self.f_dot_fun( cn, @@ -174,9 +155,6 @@ def system_dynamics( a, tau1, km, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, ) # Equation n°2 a_dot = self.a_dot_fun(a, f) # Equation n°5 tau1_dot = self.tau1_dot_fun(tau1, f) # Equation n°9 @@ -227,93 +205,3 @@ def km_dot_fun(self, km: MX, f: MX) -> MX | float: The value of the derivative cross_bridges (unitless) """ return -(km - self.km_rest) / self.tau_fat + self.alpha_km * f # Equation n°11 - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - force_length_relationship: float | MX = 1, - force_velocity_relationship: float | MX = 1, - passive_force_relationship: MX | float = 0, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system, none - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: DingModelPulseIntensityFrequencyWithFatigue - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless) - force_velocity_relationship: MX | float - The force velocity relationship value (unitless) - passive_force_relationship: MX | float - The passive force relationship value (unitless) - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - a=states[2], - tau1=states[3], - km=states[4], - t=time, - t_stim_prev=numerical_timeseries, - pulse_intensity=controls, - force_length_relationship=force_length_relationship, - force_velocity_relationship=force_velocity_relationship, - passive_force_relationship=passive_force_relationship, - ), - defects=None, - ) - - def declare_ding_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: list = (), - ): - """ - Tell the program which variables are states and controls. - The user is expected to use the ConfigureProblem.configure_xxx functions. - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node. Experimental external forces should be included here. - contact_type: list - A list of contact types. This is used to define the contact forces in the dynamics. Not used in this model. - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - StateConfigure().configure_pulse_intensity(ocp, nlp, truncation=self.sum_stim_truncation) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) diff --git a/cocofest/models/marion2009/marion2009.py b/cocofest/models/marion2009/marion2009.py index 1c5d99c4..6152fe31 100644 --- a/cocofest/models/marion2009/marion2009.py +++ b/cocofest/models/marion2009/marion2009.py @@ -2,11 +2,6 @@ from casadi import MX, vertcat - -from bioptim import ( - DynamicsEvaluation, - NonLinearProgram, -) from cocofest.models.ding2007.ding2007 import DingModelFrequency @@ -103,32 +98,35 @@ def angle_scaling_factor(self, theta: MX) -> MX: def system_dynamics( self, - cn: MX, - f: MX, - t: MX = None, - t_stim_prev: list[float] | list[MX] = None, - theta: MX = None, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics incorporating angle dependency. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[float] | list[MX] - The time list of the previous stimulations (s) - theta: MX - The current knee angle in degrees + time: MX + The system's current node time + states: MX + The state of the system CN, F + controls: MX + The controls of the system, theta + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + theta = controls[0] if controls.shape[0] > 0 else 90 + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) # Apply angle scaling @@ -144,63 +142,3 @@ def system_dynamics( ) return vertcat(cn_dot, f_dot) - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic including angle dependency - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F - controls: MX - The controls of the system: pulse_width, theta - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2009ModelFrequency - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless), not considered for this model - force_velocity_relationship: MX | float - The force velocity relationship value (unitless), not considered for this model - passive_force_relationship: MX | float - The passive force coefficient of the muscle (unitless), not considered for this model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - t=time, - t_stim_prev=numerical_timeseries, - theta=controls[0] if controls.shape[0] > 0 else 90, - ), - defects=None, - ) diff --git a/cocofest/models/marion2009/marion2009_modified.py b/cocofest/models/marion2009/marion2009_modified.py index 741d19f0..f22582a1 100644 --- a/cocofest/models/marion2009/marion2009_modified.py +++ b/cocofest/models/marion2009/marion2009_modified.py @@ -2,10 +2,6 @@ from casadi import MX, vertcat -from bioptim import ( - DynamicsEvaluation, - NonLinearProgram, -) from cocofest.models.ding2007.ding2007 import DingModelPulseWidthFrequency @@ -108,35 +104,36 @@ def angle_scaling_factor(self, theta: MX) -> MX: def system_dynamics( self, - cn: MX, - f: MX, - t: MX = None, - t_stim_prev: list[float] | list[MX] = None, - pulse_width: MX = None, - theta: MX = None, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics incorporating angle dependency. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[float] | list[MX] - The time list of the previous stimulations (s) - pulse_width: MX - The pulsation duration of the current stimulation (s) - theta: MX - The current knee angle in degrees + time: MX + The system's current node time + states: MX + The state of the system CN, F + controls: MX + The controls of the system, pulse_width, theta + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + pulse_width = controls[0] + theta=controls[1] if controls.shape[0] > 1 else 90 + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) # Calculate base a_scale with pulse width dependency @@ -155,64 +152,3 @@ def system_dynamics( ) return vertcat(cn_dot, f_dot) - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - force_length_relationship: MX | float = 1, - force_velocity_relationship: MX | float = 1, - passive_force_relationship: MX | float = 0, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic including angle dependency - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F - controls: MX - The controls of the system: pulse_width, theta - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2009ModelPulseWidthFrequency - The current phase fes model - force_length_relationship: MX | float - The force length relationship value (unitless), not considered for this model - force_velocity_relationship: MX | float - The force velocity relationship value (unitless), not considered for this model - passive_force_relationship: MX | float - The passive force coefficient of the muscle (unitless), not considered for this model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - t=time, - t_stim_prev=numerical_timeseries, - pulse_width=controls[0], - theta=controls[1] if controls.shape[0] > 1 else 90, - ), - defects=None, - ) diff --git a/cocofest/models/marion2009/marion2009_modified_with_fatigue.py b/cocofest/models/marion2009/marion2009_modified_with_fatigue.py index aa7e9e3e..acf64336 100644 --- a/cocofest/models/marion2009/marion2009_modified_with_fatigue.py +++ b/cocofest/models/marion2009/marion2009_modified_with_fatigue.py @@ -3,10 +3,6 @@ import numpy as np from casadi import MX, vertcat -from bioptim import ( - DynamicsEvaluation, - NonLinearProgram, -) from .marion2009_modified import Marion2009ModelPulseWidthFrequency @@ -107,44 +103,39 @@ def serialize(self) -> tuple[Callable, dict]: def system_dynamics( self, - cn: MX, - f: MX, - a: MX = None, - tau1: MX = None, - km: MX = None, - t: MX = None, - t_stim_prev: list[float] | list[MX] = None, - pulse_width: MX = None, - theta: MX = None, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics incorporating both angle dependency and fatigue states. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - a: MX - The value of the scaling factor (unitless) - tau1: MX - The value of the time_state_force_no_cross_bridge (ms) - km: MX - The value of the cross_bridges (unitless) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[float] | list[MX] - The time list of the previous stimulations (s) - pulse_width: MX - The pulsation duration of the current stimulation (s) - theta: MX - The current knee angle in degrees + time: MX + The system's current node time + states: MX + The state of the system CN, F + controls: MX + The controls of the system, pulse_width, theta + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + a = states[2] + tau1 = states[3] + km=states[4] + pulse_width = controls[0] + theta = controls[1] if controls.shape[0] > 1 else 90 + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) # Calculate base a_scale with pulse width dependency @@ -213,58 +204,3 @@ def km_dot_fun(self, km: MX, f: MX) -> MX | float: The value of the derivative cross_bridges (unitless) """ return -(km - self.km_rest) / self.tau_fat + self.alpha_km * f - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic including angle dependency and fatigue - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system: pulse_width, theta - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2009ModelPulseWidthFrequencyWithFatigue - The current phase fes model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - a=states[2], - tau1=states[3], - km=states[4], - t=time, - t_stim_prev=numerical_timeseries, - pulse_width=controls[0], - theta=controls[1] if controls.shape[0] > 1 else 90, - ), - defects=None, - ) diff --git a/cocofest/models/marion2009/marion2009_with_fatigue.py b/cocofest/models/marion2009/marion2009_with_fatigue.py index 948c385d..431ce752 100644 --- a/cocofest/models/marion2009/marion2009_with_fatigue.py +++ b/cocofest/models/marion2009/marion2009_with_fatigue.py @@ -3,10 +3,6 @@ import numpy as np from casadi import MX, vertcat -from bioptim import ( - DynamicsEvaluation, - NonLinearProgram, -) from .marion2009 import Marion2009ModelFrequency @@ -104,41 +100,38 @@ def serialize(self) -> tuple[Callable, dict]: def system_dynamics( self, - cn: MX, - f: MX, - a: MX = None, - tau1: MX = None, - km: MX = None, - t: MX = None, - t_stim_prev: list[float] | list[MX] = None, - theta: MX = None, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics incorporating both angle dependency and fatigue states. Parameters ---------- - cn: MX - The value of the ca_troponin_complex (unitless) - f: MX - The value of the force (N) - a: MX - The value of the scaling factor (unitless) - tau1: MX - The value of the time_state_force_no_cross_bridge (ms) - km: MX - The value of the cross_bridges (unitless) - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: list[float] | list[MX] - The time list of the previous stimulations (s) - theta: MX - The current knee angle in degrees + time: MX + The system's current node time + states: MX + The state of the system CN, F, A, Tau1, Km + controls: MX + The controls of the system, theta + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + a = states[2] + tau1 = states[3] + km = states[4] + theta = controls[0] if controls.shape[0] > 0 else 90 + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) # Apply angle scaling @@ -204,57 +197,3 @@ def km_dot_fun(self, km: MX, f: MX) -> MX | float: The value of the derivative cross_bridges (unitless) """ return -(km - self.km_rest) / self.tau_fat + self.alpha_km * f - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic including angle dependency and fatigue - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system: pulse_width, theta - parameters: MX - The parameters acting on the system, final time of each phase - algebraic_states: MX - The stochastic variables of the system, none - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2009ModelFrequencyWithFatigue - The current phase fes model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - a=states[2], - tau1=states[3], - km=states[4], - t=time, - t_stim_prev=numerical_timeseries, - theta=controls[0] if controls.shape[0] > 0 else 90, - ), - defects=None, - ) diff --git a/cocofest/models/marion2013/marion2013.py b/cocofest/models/marion2013/marion2013.py index 0ac91cf5..13095e4c 100644 --- a/cocofest/models/marion2013/marion2013.py +++ b/cocofest/models/marion2013/marion2013.py @@ -2,10 +2,6 @@ from casadi import MX, vertcat, exp, cos, pi import numpy as np -from bioptim import ( - DynamicsEvaluation, - NonLinearProgram, -) from cocofest.models.marion2009.marion2009 import Marion2009ModelFrequency @@ -126,38 +122,37 @@ def calculate_acceleration(self, theta: MX, lambda_angle: MX, f: MX, Fload: MX) def system_dynamics( self, - cn: MX, - f: MX, - theta: MX, - dtheta_dt: MX, - t: MX = None, - t_stim_prev: MX = None, - Fload: MX = 0.0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics that describes the model. Parameters ---------- - cn: MX - The normalized Ca2+-troponin complex concentration - f: MX - The instantaneous force - theta: MX - The flexion angle - dtheta_dt: MX - The angular velocity - t: MX - The current time at which the dynamics is evaluated (ms) - t_stim_prev: MX - The time of the previous stimulation (ms) - Fload: MX - External load force (N) + time: MX + The system's current node time + states: MX + The state of the system CN, F, theta, dtheta + controls: MX + The controls of the system, Fload + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + theta = states[2] + dtheta_dt = states[3] + Fload = controls[0] if controls.shape[0] > 0 else 0.0 + t_stim_prev = numerical_timeseries + cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) # Similar to Ding's model calculation # Calculate Marion-specific force scaling terms @@ -182,56 +177,3 @@ def system_dynamics( Fload, ) return vertcat(cn_dot, f_dot, dtheta_dt, d2theta_dt2) - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, theta, dtheta_dt - controls: MX - The controls of the system - parameters: MX - The parameters acting on the system - algebraic_states: MX - The stochastic variables of the system - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2013ModelFrequency - The current phase fes model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - theta=states[2], - dtheta_dt=states[3], - t=time, - t_stim_prev=numerical_timeseries, - Fload=controls[0] if controls.shape[0] > 0 else 0.0, - ), - defects=None, - ) diff --git a/cocofest/models/marion2013/marion2013_modified.py b/cocofest/models/marion2013/marion2013_modified.py index 3004f758..766217c7 100644 --- a/cocofest/models/marion2013/marion2013_modified.py +++ b/cocofest/models/marion2013/marion2013_modified.py @@ -130,41 +130,38 @@ def calculate_acceleration(self, theta: MX, lambda_angle: MX, f: MX, Fload: MX) def system_dynamics( self, - cn: MX, - f: MX, - theta: MX, - dtheta_dt: MX, - t: MX = None, - t_stim_prev: MX = None, - pulse_width: MX = None, - Fload: MX = 0.0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics that describes the model. Parameters ---------- - cn: MX - The normalized Ca2+-troponin complex concentration - f: MX - The instantaneous force - theta: MX - The flexion angle - dtheta_dt: MX - The angular velocity - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: MX - The time of the previous stimulation (s) - pulse_width: MX - The pulse width of the stimulation (s) - Fload: MX - External load force (N) + time: MX + The system's current node time + states: MX + The state of the system CN, F, theta, dtheta + controls: MX + The controls of the system, pulse_width, Fload + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + theta = states[2] + dtheta_dt = states[3] + pulse_width = controls[0] + Fload = controls[1] if controls.shape[0] > 1 else 0.0 + t_stim_prev = numerical_timeseries + # Get CN dynamics from Ding model cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) @@ -194,57 +191,3 @@ def system_dynamics( ) return vertcat(cn_dot, f_dot, dtheta_dt, d2theta_dt2) - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, theta, dtheta_dt - controls: MX - The controls of the system - parameters: MX - The parameters acting on the system - algebraic_states: MX - The stochastic variables of the system - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2013ModelPulseWidthFrequency - The current phase fes model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - theta=states[2], - dtheta_dt=states[3], - t=time, - t_stim_prev=numerical_timeseries, - pulse_width=controls[0], - Fload=controls[1] if controls.shape[0] > 1 else 0.0, - ), - defects=None, - ) diff --git a/cocofest/models/marion2013/marion2013_modified_with_fatigue.py b/cocofest/models/marion2013/marion2013_modified_with_fatigue.py index 282b7842..332c317c 100644 --- a/cocofest/models/marion2013/marion2013_modified_with_fatigue.py +++ b/cocofest/models/marion2013/marion2013_modified_with_fatigue.py @@ -2,10 +2,6 @@ from casadi import MX, vertcat import numpy as np -from bioptim import ( - DynamicsEvaluation, - NonLinearProgram, -) from .marion2013_modified import Marion2013ModelPulseWidthFrequency @@ -166,50 +162,41 @@ def km_dot_fun(self, km: MX, f: MX, velocity: MX) -> MX | float: def system_dynamics( self, - cn: MX, - f: MX, - theta: MX, - dtheta_dt: MX, - a: MX, - tau1: MX, - km: MX, - t: MX = None, - t_stim_prev: MX = None, - pulse_width: MX = None, - Fload: MX = 0.0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics including fatigue effects. Parameters ---------- - cn: MX - The normalized Ca2+-troponin complex concentration - f: MX - The instantaneous force near ankle - theta: MX - The knee flexion angle - dtheta_dt: MX - The angular velocity - a: MX - The current a value at 90° affected by fatigue - tau1: MX - The current tau1 value affected by fatigue - km: MX - The current Km value affected by fatigue - t: MX - The current time at which the dynamics is evaluated (s) - t_stim_prev: MX - The time of the previous stimulation (s) - pulse_width: MX - The pulse width of the stimulation (s) - Fload: MX - External load force (N) + time: MX + The system's current node time + states: MX + The state of the system CN, F, theta, dtheta, A, Tau1, Km + controls: MX + The controls of the system, pulse_width, Fload + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn = states[0] + f = states[1] + theta = states[2] + dtheta_dt = states[3] + a = states[4] + tau1 = states[5] + km = states[6] + pulse_width = controls[0] + Fload = controls[1] if controls.shape[0] > 1 else 0.0 + t_stim_prev = numerical_timeseries + # Get CN dynamics from Ding model cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) @@ -244,60 +231,3 @@ def system_dynamics( tau1_dot = self.tau1_dot_fun(tau1, f, dtheta_dt) return vertcat(cn_dot, f_dot, dtheta_dt, d2theta_dt2, a_dot, tau1_dot, km_dot) - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic with fatigue - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system including fatigue states - controls: MX - The controls of the system - parameters: MX - The parameters acting on the system - algebraic_states: MX - The stochastic variables of the system - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2013ModelFrequencyWithFatigue - The current phase fes model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - theta=states[2], - dtheta_dt=states[3], - a=states[4], - tau1=states[5], - km=states[6], - t=time, - t_stim_prev=numerical_timeseries, - pulse_width=controls[0], - Fload=controls[1] if controls.shape[0] > 1 else 0.0, - ), - defects=None, - ) diff --git a/cocofest/models/marion2013/marion2013_with_fatigue.py b/cocofest/models/marion2013/marion2013_with_fatigue.py index 2cf11dce..0eed3a81 100644 --- a/cocofest/models/marion2013/marion2013_with_fatigue.py +++ b/cocofest/models/marion2013/marion2013_with_fatigue.py @@ -2,10 +2,6 @@ from casadi import MX, vertcat import numpy as np -from bioptim import ( - DynamicsEvaluation, - NonLinearProgram, -) from .marion2013 import Marion2013ModelFrequency @@ -163,47 +159,40 @@ def km_dot_fun(self, km: MX, f: MX, velocity: MX) -> MX | float: def system_dynamics( self, - cn: MX, - f: MX, - theta: MX, - dtheta_dt: MX, - a: MX, - tau1: MX, - km: MX, - t: MX = None, - t_stim_prev: MX = None, - Fload: MX = 0.0, + time: MX, + states: MX, + controls: MX, + numerical_timeseries: MX, ) -> MX: """ The system dynamics including fatigue effects. Parameters ---------- - cn: MX - The normalized Ca2+-troponin complex concentration - f: MX - The instantaneous force near ankle - theta: MX - The knee flexion angle - dtheta_dt: MX - The angular velocity - a: MX - The current a value at 90° affected by fatigue - km: MX - The current Km value affected by fatigue - tau1: MX - The current tau1 value affected by fatigue - t: MX - The current time at which the dynamics is evaluated (ms) - t_stim_prev: MX - The time of the previous stimulation (ms) - Fload: MX - External load force (N) + time: MX + The system's current node time + states: MX + The state of the system CN, F, theta, dtheta, A, Tau1, Km + controls: MX + The controls of the system, Fload + numerical_timeseries: MX + The numerical timeseries of the system Returns ------- The value of the derivative of each state dx/dt at the current time t """ + t = time + cn=states[0] + f=states[1] + theta=states[2] + dtheta_dt=states[3] + a=states[4] + tau1=states[5] + km=states[6] + Fload=controls[0] if controls.shape[0] > 0 else 0.0 + t_stim_prev = numerical_timeseries + # Get CN dynamics from Ding model cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) @@ -235,59 +224,3 @@ def system_dynamics( tau1_dot = self.tau1_dot_fun(tau1, f, dtheta_dt) return vertcat(cn_dot, f_dot, dtheta_dt, d2theta_dt2, a_dot, tau1_dot, km_dot) - - @staticmethod - def dynamics( - time: MX, - states: MX, - controls: MX, - parameters: MX, - algebraic_states: MX, - numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: - """ - Functional electrical stimulation dynamic with fatigue - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system including fatigue states - controls: MX - The controls of the system - parameters: MX - The parameters acting on the system - algebraic_states: MX - The stochastic variables of the system - numerical_timeseries: MX - The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: Marion2013ModelFrequencyWithFatigue - The current phase fes model - - Returns - ------- - The derivative of the states in the tuple[MX] format - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - cn=states[0], - f=states[1], - theta=states[2], - dtheta_dt=states[3], - a=states[4], - tau1=states[5], - km=states[6], - t=time, - t_stim_prev=numerical_timeseries, - Fload=controls[0] if controls.shape[0] > 0 else 0.0, - ), - defects=None, - ) diff --git a/cocofest/models/state_configure.py b/cocofest/models/state_configure.py index cd594343..2b491545 100644 --- a/cocofest/models/state_configure.py +++ b/cocofest/models/state_configure.py @@ -1,5 +1,5 @@ from bioptim import ( - ConfigureProblem, + ConfigureVariables, NonLinearProgram, OptimalControlProgram, ) @@ -49,7 +49,7 @@ def configure_ca_troponin_complex( muscle_name = "_" + muscle_name if muscle_name else "" name = "Cn" + muscle_name name_cn = [name] - ConfigureProblem.configure_new_variable( + ConfigureVariables.configure_new_variable( name, name_cn, ocp, @@ -89,7 +89,7 @@ def configure_force( muscle_name = "_" + muscle_name if muscle_name else "" name = "F" + muscle_name name_f = [name] - ConfigureProblem.configure_new_variable( + ConfigureVariables.configure_new_variable( name, name_f, ocp, @@ -129,7 +129,7 @@ def configure_scaling_factor( muscle_name = "_" + muscle_name if muscle_name else "" name = "A" + muscle_name name_a = [name] - return ConfigureProblem.configure_new_variable( + return ConfigureVariables.configure_new_variable( name, name_a, ocp, @@ -169,7 +169,7 @@ def configure_time_state_force_no_cross_bridge( muscle_name = "_" + muscle_name if muscle_name else "" name = "Tau1" + muscle_name name_tau1 = [name] - return ConfigureProblem.configure_new_variable( + return ConfigureVariables.configure_new_variable( name, name_tau1, ocp, @@ -209,7 +209,7 @@ def configure_cross_bridges( muscle_name = "_" + muscle_name if muscle_name else "" name = "Km" + muscle_name name_km = [name] - return ConfigureProblem.configure_new_variable( + return ConfigureVariables.configure_new_variable( name, name_km, ocp, @@ -236,7 +236,7 @@ def configure_cn_sum(ocp, nlp, muscle_name: str = None): muscle_name = "_" + muscle_name if muscle_name else "" name = "Cn_sum" + muscle_name name_cn_sum = [name] - return ConfigureProblem.configure_new_variable(name, name_cn_sum, ocp, nlp, as_states=False, as_controls=True) + return ConfigureVariables.configure_new_variable(name, name_cn_sum, ocp, nlp, as_states=False, as_controls=True) @staticmethod def configure_a_calculation(ocp, nlp, muscle_name: str = None): @@ -255,7 +255,7 @@ def configure_a_calculation(ocp, nlp, muscle_name: str = None): muscle_name = "_" + muscle_name if muscle_name else "" name = "A_calculation" + muscle_name name_cn_sum = [name] - return ConfigureProblem.configure_new_variable(name, name_cn_sum, ocp, nlp, as_states=False, as_controls=True) + return ConfigureVariables.configure_new_variable(name, name_cn_sum, ocp, nlp, as_states=False, as_controls=True) @staticmethod def configure_muscle_activation( @@ -287,7 +287,7 @@ def configure_muscle_activation( muscle_name = "_" + muscle_name if muscle_name else "" name = "a" + muscle_name name_a = [name] - ConfigureProblem.configure_new_variable( + ConfigureVariables.configure_new_variable( name, name_a, ocp, @@ -327,7 +327,7 @@ def configure_fatigue_state( muscle_name = "_" + muscle_name if muscle_name else "" name = "mu" + muscle_name name_mu = [name] - ConfigureProblem.configure_new_variable( + ConfigureVariables.configure_new_variable( name, name_mu, ocp, @@ -367,7 +367,7 @@ def configure_angle( muscle_name = "_" + muscle_name if muscle_name else "" name = "theta" + muscle_name name_theta = [name] - ConfigureProblem.configure_new_variable( + ConfigureVariables.configure_new_variable( name, name_theta, ocp, @@ -407,7 +407,7 @@ def configure_angular_velocity( muscle_name = "_" + muscle_name if muscle_name else "" name = "dtheta_dt" + muscle_name name_dtheta_dt = [name] - ConfigureProblem.configure_new_variable( + ConfigureVariables.configure_new_variable( name, name_dtheta_dt, ocp, @@ -418,7 +418,7 @@ def configure_angular_velocity( ) @staticmethod - def configure_last_pulse_width(ocp, nlp, muscle_name: str = None): + def configure_last_pulse_width(ocp, nlp, muscle_name: str = None, as_states=True, as_controls=False, as_algebraic_states=False): """ Configure the last pulse width control @@ -432,12 +432,12 @@ def configure_last_pulse_width(ocp, nlp, muscle_name: str = None): muscle_name = "_" + muscle_name if muscle_name else "" name = "last_pulse_width" + muscle_name last_pulse_width = [name] - return ConfigureProblem.configure_new_variable( + return ConfigureVariables.configure_new_variable( name, last_pulse_width, ocp, nlp, as_states=False, as_controls=True ) @staticmethod - def configure_pulse_intensity(ocp, nlp, muscle_name: str = None, truncation: int = 20): + def configure_pulse_intensity(ocp, nlp, muscle_name: str = None, truncation: int = None, as_states=True, as_controls=False, as_algebraic_states=False): """ Configure the pulse intensity control for the Ding model @@ -448,15 +448,17 @@ def configure_pulse_intensity(ocp, nlp, muscle_name: str = None, truncation: int nlp: NonLinearProgram A reference to the phase """ + muscle_name = nlp.model.muscle_name if muscle_name is None else muscle_name + truncation = nlp.model.sum_stim_truncation if truncation is None else truncation muscle_name = "_" + muscle_name if muscle_name else "" name = "pulse_intensity" + muscle_name pulse_intensity = [str(i) for i in range(truncation)] - return ConfigureProblem.configure_new_variable( + return ConfigureVariables.configure_new_variable( name, pulse_intensity, ocp, nlp, as_states=False, as_controls=True ) @staticmethod - def configure_intensity(ocp, nlp, muscle_name: str = None): + def configure_intensity(ocp, nlp, as_states=False, as_controls=True, as_algebraic_states=False): """ Configure the intensity control for the Veltink1992 model @@ -467,28 +469,38 @@ def configure_intensity(ocp, nlp, muscle_name: str = None): nlp: NonLinearProgram A reference to the phase """ + muscle_name = nlp.model.muscle_name muscle_name = "_" + muscle_name if muscle_name else "" name = "I" + muscle_name pulse_intensity = [name] - return ConfigureProblem.configure_new_variable( + return ConfigureVariables.configure_new_variable( name, pulse_intensity, ocp, nlp, as_states=False, as_controls=True ) - def configure_all_muscle_states(self, muscles_dynamics_model, ocp, nlp): - state_name_list = [] - for muscle_dynamics_model in muscles_dynamics_model: + @staticmethod + def configure_all_muscle_states(ocp, nlp, as_states=True, as_controls=False, as_algebraic_states=False): + for state_key in nlp.model.name_dof: + if state_key in StateConfigure().state_dictionary.keys(): + StateConfigure().state_dictionary[state_key]( + ocp=ocp, + nlp=nlp, + as_states=True, + as_controls=False, + muscle_name=nlp.model.muscle_name, + ) + + @staticmethod + def configure_all_muscle_msk_states(ocp, nlp, as_states=True, as_controls=False, as_algebraic_states=False): + for muscle_dynamics_model in nlp.model.muscles_dynamics_model: for state_key in muscle_dynamics_model.name_dof: - if state_key in self.state_dictionary.keys(): - self.state_dictionary[state_key]( + if state_key in StateConfigure().state_dictionary.keys(): + StateConfigure().state_dictionary[state_key]( ocp=ocp, nlp=nlp, as_states=True, as_controls=False, muscle_name=muscle_dynamics_model.muscle_name, ) - state_name_list.append(state_key + "_" + muscle_dynamics_model.muscle_name) - - return state_name_list def configure_all_fes_model_states(self, ocp, nlp, fes_model): for state_key in fes_model.name_dof: diff --git a/cocofest/models/veltink1992/veltink1992.py b/cocofest/models/veltink1992/veltink1992.py index b5f8d412..0dc66979 100644 --- a/cocofest/models/veltink1992/veltink1992.py +++ b/cocofest/models/veltink1992/veltink1992.py @@ -2,17 +2,12 @@ from casadi import MX, vertcat import numpy as np -from bioptim import ( - ConfigureProblem, - DynamicsEvaluation, - NonLinearProgram, - OptimalControlProgram, -) +from bioptim import StateDynamics, FcnEnum from cocofest.models.state_configure import StateConfigure -class VeltinkModelPulseIntensity: +class VeltinkModelPulseIntensity(StateDynamics): """ This is a custom model implementing the muscle activation dynamics from: @@ -44,6 +39,10 @@ def __init__( self.I_threshold = I_threshold if I_threshold is not None else I_THRESHOLD_DEFAULT self.I_saturation = I_saturation if I_saturation is not None else I_SATURATION_DEFAULT + self.contact_types = () + self.state_configuration = [CustomStates.my_muscle_states] + self.control_configuration = [CustomStates.my_control] + @property def name_dof(self, with_muscle_name: bool = False) -> list[str]: muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" @@ -128,95 +127,38 @@ def get_muscle_activation(self, a: MX, u: MX) -> MX: def system_dynamics( self, - a: MX, - I: MX, - ) -> MX: - """ - The system dynamics implementing equation (4) for muscle activation. - - Parameters - ---------- - a: MX - Muscle activation state (unitless) - I: MX - Stimulation current amplitude (mA) - - Returns - ------- - The derivative of muscle activation state - """ - u = self.normalize_current(I) - a_dot = self.get_muscle_activation(a=a, u=u) - - return vertcat(a_dot) - - @staticmethod - def dynamics( time: MX, states: MX, controls: MX, - parameters: MX, - algebraic_states: MX, numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: + ) -> MX: """ + The system dynamics implementing equation (4) for muscle activation. + Parameters ---------- time: MX The system's current node time states: MX - The state of the system (muscle activation) + The state of the system a controls: MX - The controls of the system (stimulation current) - parameters: MX - The parameters acting on the system - algebraic_states: MX - The stochastic variables of the system + The controls of the system, I numerical_timeseries: MX The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: VeltinkModelPulseIntensity - The current phase fes model Returns ------- - The derivative of the states + The derivative of muscle activation state """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - a=states[0], - I=controls[0], - ), - defects=None, - ) + a = states[0] + I = controls[0] + u = self.normalize_current(I) + a_dot = self.get_muscle_activation(a=a, u=u) - def declare_model_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: tuple = (), - ): - """ - Tell the program which variables are states and controls. + return vertcat(a_dot) - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node - contact_type: tuple - The type of contact to use for the model - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - StateConfigure().configure_intensity(ocp, nlp) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) + +# TODO: Change to future bioptim version +class CustomStates(FcnEnum): + my_muscle_states = (StateConfigure().configure_all_muscle_states,) + my_control = (StateConfigure().configure_intensity,) diff --git a/cocofest/models/veltink1992/veltink1992_and_riener1998.py b/cocofest/models/veltink1992/veltink1992_and_riener1998.py index 1ad277df..3bbe44bd 100644 --- a/cocofest/models/veltink1992/veltink1992_and_riener1998.py +++ b/cocofest/models/veltink1992/veltink1992_and_riener1998.py @@ -2,15 +2,7 @@ from casadi import MX, vertcat import numpy as np -from bioptim import ( - ConfigureProblem, - DynamicsEvaluation, - NonLinearProgram, - OptimalControlProgram, -) - from cocofest.models.veltink1992.veltink1992 import VeltinkModelPulseIntensity -from cocofest.models.state_configure import StateConfigure class VeltinkRienerModelPulseIntensityWithFatigue(VeltinkModelPulseIntensity): @@ -118,100 +110,34 @@ def get_mu_dot(self, a: MX, mu: MX) -> MX: def system_dynamics( self, - a: MX, - mu: MX, - I: MX, - ) -> MX: - """ - The system dynamics including fatigue effects. - - Parameters - ---------- - a: MX - Muscle activation state (unitless) - mu: MX - Fatigue state (unitless) - I: MX - Stimulation current amplitude (mA) - - Returns - ------- - The value of the derivative of each state - """ - u = self.normalize_current(I) - a_dot = self.get_muscle_activation(a=a, u=u) - mu_dot = self.get_mu_dot(a=a, mu=mu) - - return vertcat(a_dot, mu_dot) - - @staticmethod - def dynamics( time: MX, states: MX, controls: MX, - parameters: MX, - algebraic_states: MX, numerical_timeseries: MX, - nlp: NonLinearProgram, - fes_model=None, - ) -> DynamicsEvaluation: + ) -> MX: """ + The system dynamics including fatigue effects. + Parameters ---------- time: MX The system's current node time states: MX - The state of the system (muscle activation, fatigue) + The state of the system a, mu controls: MX - The controls of the system (stimulation current) - parameters: MX - The parameters acting on the system - algebraic_states: MX - The stochastic variables of the system + The controls of the system, I numerical_timeseries: MX The numerical timeseries of the system - nlp: NonLinearProgram - A reference to the phase - fes_model: VeltinkRienerModelPulseIntensityWithFatigue - The current phase fes model Returns ------- - The derivative of the states - """ - model = fes_model if fes_model else nlp.model - dxdt_fun = model.system_dynamics - - return DynamicsEvaluation( - dxdt=dxdt_fun( - a=states[0], - mu=states[1], - I=controls[0], - ), - defects=None, - ) - - def declare_model_variables( - self, - ocp: OptimalControlProgram, - nlp: NonLinearProgram, - numerical_data_timeseries: dict[str, np.ndarray] = None, - contact_type: tuple = (), - ): + The value of the derivative of each state """ - Tell the program which variables are states and controls. + a = states[0] + mu = states[1] + I = controls[0] + u = self.normalize_current(I) + a_dot = self.get_muscle_activation(a=a, u=u) + mu_dot = self.get_mu_dot(a=a, mu=mu) - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase - numerical_data_timeseries: dict[str, np.ndarray] - A list of values to pass to the dynamics at each node - contact_type: tuple - The type of contact to use for the model - """ - StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) - StateConfigure().configure_intensity(ocp, nlp) - ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) + return vertcat(a_dot, mu_dot) diff --git a/cocofest/optimization/fes_ocp.py b/cocofest/optimization/fes_ocp.py index 0e8b2754..1c43dd1a 100644 --- a/cocofest/optimization/fes_ocp.py +++ b/cocofest/optimization/fes_ocp.py @@ -3,7 +3,8 @@ from bioptim import ( BoundsList, ConstraintList, - DynamicsList, + DynamicsOptionsList, + DynamicsOptions, InitialGuessList, InterpolationType, Node, @@ -12,7 +13,6 @@ ParameterList, PhaseDynamics, VariableScaling, - OdeSolver, ) from ..fourier_approx import FourierSeries @@ -72,17 +72,18 @@ def set_constraints(model, n_shooting, stim_idx_at_node_list): return constraints @staticmethod - def declare_dynamics(model, numerical_data_timeseries=None, ode_solver=OdeSolver.RK4(n_integration_steps=10)): - dynamics = DynamicsList() - dynamics.add( - model.declare_ding_variables, - dynamic_function=model.dynamics, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_data_timeseries, - ode_solver=ode_solver, + def declare_dynamics_options(numerical_time_series, ode_solver): + dynamics_options = DynamicsOptionsList() + dynamics_options.add( + DynamicsOptions( + expand_dynamics=True, + expand_continuity=False, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + ode_solver=ode_solver, + numerical_data_timeseries=numerical_time_series, + ) ) - return dynamics + return dynamics_options @staticmethod def set_x_bounds(model): diff --git a/cocofest/optimization/fes_ocp_multibody.py b/cocofest/optimization/fes_ocp_multibody.py index b76beefa..ba8d7e7b 100644 --- a/cocofest/optimization/fes_ocp_multibody.py +++ b/cocofest/optimization/fes_ocp_multibody.py @@ -3,12 +3,10 @@ from bioptim import ( BoundsList, ConstraintList, - DynamicsList, ExternalForceSetTimeSeries, InitialGuessList, InterpolationType, ParameterList, - PhaseDynamics, VariableScaling, ) @@ -38,22 +36,6 @@ def get_numerical_time_series_for_external_forces(n_shooting, external_force_dic return numerical_time_series, external_force_set - @staticmethod - def declare_dynamics(bio_models, numerical_time_series, ode_solver, contact_type): - dynamics = DynamicsList() - dynamics.add( - bio_models.declare_model_variables, - dynamic_function=bio_models.muscle_dynamic, - expand_dynamics=True, - expand_continuity=False, - phase=0, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - contact_type=contact_type, - ode_solver=ode_solver, - ) - return dynamics - @staticmethod def build_parameters( model: FesMskModel, @@ -286,6 +268,7 @@ def update_model(model, parameters, external_force_set): previous_stim=model.muscles_dynamics_model[0].previous_stim, activate_force_length_relationship=model.activate_force_length_relationship, activate_force_velocity_relationship=model.activate_force_velocity_relationship, + activate_passive_force_relationship=model.activate_passive_force_relationship, activate_residual_torque=model.activate_residual_torque, parameters=parameters, external_force_set=external_force_set, diff --git a/examples/fes_multibody/cycling/cycling_bayesian_mhe.py b/examples/fes_multibody/cycling/cycling_bayesian_mhe.py index 06b6069f..9ea09f99 100644 --- a/examples/fes_multibody/cycling/cycling_bayesian_mhe.py +++ b/examples/fes_multibody/cycling/cycling_bayesian_mhe.py @@ -117,7 +117,9 @@ def prepare_nmpc_bo( numerical_time_series.update(time_series2) # --- Dynamics & states --- # - dynamics = base.set_dynamics(model=model, numerical_time_series=numerical_time_series, ode_solver=ode_solver) + dynamics_options = base.set_dynamics_options(numerical_time_series=numerical_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) + x_init = base.set_q_qdot_init( n_shooting=window_n_shooting, pedal_config=cycling_info["pedal_config"], @@ -148,7 +150,7 @@ def prepare_nmpc_bo( return base.MyCyclicNMPC( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, cycle_len=cycle_len, cycle_duration=cycle_duration, n_cycles_simultaneous=n_cycles_simultaneous, diff --git a/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py b/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py index d5fe7c34..d727d438 100644 --- a/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py +++ b/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py @@ -18,9 +18,7 @@ BoundsList, ConstraintList, ConstraintFcn, - ContactType, CostType, - DynamicsList, ExternalForceSetTimeSeries, InitialGuessList, InterpolationType, @@ -29,7 +27,6 @@ ObjectiveFcn, ObjectiveList, OdeSolver, - PhaseDynamics, SolutionMerge, Solution, Solver, @@ -333,7 +330,7 @@ def prepare_nmpc( ) numerical_time_series.update(numerical_data_time_series) # --- Dynamics --- # - dynamics = set_dynamics(model=model, numerical_time_series=numerical_time_series, ode_solver=ode_solver) + dynamics_options = set_dynamics_options(numerical_time_series=numerical_time_series, ode_solver=ode_solver) # --- Set states --- # # --- Set q (position and speed) initial guesses --- # @@ -377,7 +374,7 @@ def prepare_nmpc( return MyCyclicNMPC( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, cycle_len=cycle_len, cycle_duration=cycle_duration, n_cycles_simultaneous=n_cycles_simultaneous, @@ -406,19 +403,10 @@ def set_external_forces(n_shooting, external_force_dict, force_name): return numerical_time_series, external_force_set -def set_dynamics(model, numerical_time_series, ode_solver): - dynamics = DynamicsList() - dynamics.add( - dynamics_type=model.declare_model_variables, - dynamic_function=model.muscle_dynamic, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - contact_type=[ContactType.RIGID_EXPLICIT], - phase=0, - ode_solver=ode_solver, - ) - return dynamics +def set_dynamics_options(numerical_time_series, ode_solver): + dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_time_series, + ode_solver=ode_solver) + return dynamics_options def set_q_qdot_init( diff --git a/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py b/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py index 016099f9..53ec3e5f 100644 --- a/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py +++ b/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py @@ -14,8 +14,6 @@ ConstraintFcn, ControlType, CostType, - DynamicsFcn, - DynamicsList, ExternalForceSetTimeSeries, InitialGuessList, InterpolationType, diff --git a/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py b/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py index 18674678..f327cb7b 100644 --- a/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py +++ b/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py @@ -42,13 +42,8 @@ def prepare_ocp( numerical_data_time_series, stim_idx_at_node_list = muscle_model.get_numerical_data_time_series( n_shooting, final_time ) - - dynamics = OcpFesMsk.declare_dynamics( - model, - numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10), - contact_type=[], - ) + dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) u_bounds, u_init = OcpFesMsk.set_u_bounds(model, msk_info["with_residual_torque"], max_bound=max_bound) @@ -99,7 +94,7 @@ def prepare_ocp( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, @@ -139,6 +134,7 @@ def main(plot=True, model_path="../../msk_models/Arm26/arm26_biceps_triceps.bioM activate_passive_force_relationship=True, activate_residual_torque=False, stim_time=stim_time, + with_contact=False, ) final_time = 1 diff --git a/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py b/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py index 2535a648..3a8eebda 100644 --- a/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py +++ b/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py @@ -21,13 +21,8 @@ def prepare_ocp(model, final_time: float, resistive_torque, msk_info): n_shooting, final_time ) numerical_time_series.update(numerical_data_time_series) - - dynamics = OcpFesMsk.declare_dynamics( - model, - numerical_time_series=numerical_time_series, - contact_type=[], - ode_solver=OdeSolver.RK4(n_integration_steps=10), - ) + dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) # --- Set initial guesses and bounds for states and controls --- # x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) @@ -48,11 +43,12 @@ def prepare_ocp(model, final_time: float, resistive_torque, msk_info): activate_force_velocity_relationship=model.activate_force_velocity_relationship, activate_residual_torque=model.activate_residual_torque, external_force_set=external_force_set, + with_contact=False, ) return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/fes_multibody/reaching/reaching_task.py b/examples/fes_multibody/reaching/reaching_task.py index 3a131ba0..e1040f75 100644 --- a/examples/fes_multibody/reaching/reaching_task.py +++ b/examples/fes_multibody/reaching/reaching_task.py @@ -52,6 +52,7 @@ def initialize_model(final_time, model_path): activate_force_velocity_relationship=True, activate_passive_force_relationship=True, activate_residual_torque=False, + with_contact=False, ) return model @@ -66,13 +67,8 @@ def prepare_ocp( numerical_data_time_series, stim_idx_at_node_list = muscle_model.get_numerical_data_time_series( n_shooting, final_time ) - - dynamics = OcpFesMsk.declare_dynamics( - model, - numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau"), - contact_type=[], - ) + dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau")) # --- Initialize default FES bounds and initial guess --- # x_bounds, x_init_fes = OcpFesMsk.set_x_bounds_fes(model) @@ -124,7 +120,7 @@ def prepare_ocp( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, @@ -152,11 +148,3 @@ def main(plot=True, model_path="../../msk_models/Arm26/arm26_with_reaching_targe if __name__ == "__main__": main() - -# [1] Dahmane, R., Djordjevič, S., Šimunič, B., & Valenčič, V. (2005). -# Spatial fiber type distribution in normal human muscle: histochemical and tensiomyographical evaluation. -# Journal of biomechanics, 38(12), 2451-2459. - -# [2] Klein, C. S., Allman, B. L., Marsh, G. D., & Rice, C. L. (2002). -# Muscle size, strength, and bone geometry in the upper limbs of young and old men. -# The Journals of Gerontology Series A: Biological Sciences and Medical Sciences, 57(7), M455-M459. diff --git a/examples/getting_started/identification/muscle_model_id.py b/examples/getting_started/identification/muscle_model_id.py index d3910f7e..eab29c7c 100644 --- a/examples/getting_started/identification/muscle_model_id.py +++ b/examples/getting_started/identification/muscle_model_id.py @@ -7,6 +7,7 @@ DingModelPulseIntensityFrequency, IvpFes, OcpFesId, + OcpFes, ) from cocofest.identification.identification_method import DataExtraction @@ -71,11 +72,8 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFesId.declare_dynamics( - model=model, - numerical_data_timeseries=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10), - ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, @@ -107,7 +105,7 @@ def prepare_ocp( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, x_init=x_init, diff --git a/examples/getting_started/multibody/pulse_intensity_optimization_multibody.py b/examples/getting_started/multibody/pulse_intensity_optimization_multibody.py index a429804f..a4dff8f7 100644 --- a/examples/getting_started/multibody/pulse_intensity_optimization_multibody.py +++ b/examples/getting_started/multibody/pulse_intensity_optimization_multibody.py @@ -34,11 +34,9 @@ def prepare_ocp(model: FesMskModel, final_time: float, external_force: dict, msk else: numerical_time_series = numerical_data_time_series - dynamics = OcpFesMsk.declare_dynamics( - model, + dynamics_options = OcpFesMsk.declare_dynamics_options( numerical_time_series=numerical_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10), - contact_type=[], ) x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) @@ -77,7 +75,7 @@ def prepare_ocp(model: FesMskModel, final_time: float, external_force: dict, msk return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/getting_started/multibody/pulse_width_optimization_multibody.py b/examples/getting_started/multibody/pulse_width_optimization_multibody.py index 10dc8110..cd67a5d8 100644 --- a/examples/getting_started/multibody/pulse_width_optimization_multibody.py +++ b/examples/getting_started/multibody/pulse_width_optimization_multibody.py @@ -35,11 +35,9 @@ def prepare_ocp(model: FesMskModel, final_time: float, external_force: dict, msk else: numerical_time_series = numerical_data_time_series - dynamics = OcpFesMsk.declare_dynamics( - model, + dynamics_options = OcpFesMsk.declare_dynamics_options( numerical_time_series=numerical_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10), - contact_type=[], ) x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) @@ -57,7 +55,7 @@ def prepare_ocp(model: FesMskModel, final_time: float, external_force: dict, msk return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, @@ -83,6 +81,7 @@ def main(plot=True, biorbd_path="../../msk_models/Arm26/arm26_biceps_1dof.bioMod activate_passive_force_relationship=True, activate_residual_torque=False, external_force_set=None, # External forces will be added later + with_contact=False, ) resistive_torque = { diff --git a/examples/getting_started/optimization/force_tracking.py b/examples/getting_started/optimization/force_tracking.py index 77de4c06..ae20957f 100644 --- a/examples/getting_started/optimization/force_tracking.py +++ b/examples/getting_started/optimization/force_tracking.py @@ -35,13 +35,10 @@ def prepare_ocp(model: FesModel, final_time: float, pw_max: float, force_trackin time_series, stim_idx_at_node_list = model.get_numerical_data_time_series( n_shooting, final_time ) # Retrieve time and indexes at which occurs the stimulation for the FES dynamic - dynamics = OcpFes.declare_dynamics( - model, - time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10), - # ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau"), # Possibility to use a different solver - ) - + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10) + # ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau"), # Possibility to use a different solver) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) x_init = OcpFes.set_x_init(model) @@ -61,7 +58,7 @@ def prepare_ocp(model: FesModel, final_time: float, pw_max: float, force_trackin return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/getting_started/optimization/frequency_optimization.py b/examples/getting_started/optimization/frequency_optimization.py index dc5f4da5..d38d6ee9 100644 --- a/examples/getting_started/optimization/frequency_optimization.py +++ b/examples/getting_started/optimization/frequency_optimization.py @@ -4,7 +4,13 @@ No optimization will be done on the stimulation, the frequency is fixed to 1Hz. """ -from bioptim import ObjectiveList, ObjectiveFcn, OptimalControlProgram, ControlType, OdeSolver, Node +from bioptim import (ObjectiveList, + ObjectiveFcn, + OptimalControlProgram, + ControlType, + OdeSolver, + Node, + ) from cocofest import OcpFes, ModelMaker @@ -12,9 +18,9 @@ def prepare_ocp(model, final_time): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFes.declare_dynamics( - model, numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) - ) + + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) # --- Set initial guesses and bounds for states --- # x_bounds = OcpFes.set_x_bounds(model) @@ -28,7 +34,7 @@ def prepare_ocp(model, final_time): return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/getting_started/optimization/pulse_intensity_optimization.py b/examples/getting_started/optimization/pulse_intensity_optimization.py index 40307edc..126a0929 100644 --- a/examples/getting_started/optimization/pulse_intensity_optimization.py +++ b/examples/getting_started/optimization/pulse_intensity_optimization.py @@ -17,9 +17,8 @@ def prepare_ocp(model, final_time, pi_max): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFes.declare_dynamics( - model, numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) - ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) @@ -51,7 +50,7 @@ def prepare_ocp(model, final_time, pi_max): return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/getting_started/optimization/pulse_width_optimization.py b/examples/getting_started/optimization/pulse_width_optimization.py index 0cc00f22..64ffc77e 100644 --- a/examples/getting_started/optimization/pulse_width_optimization.py +++ b/examples/getting_started/optimization/pulse_width_optimization.py @@ -13,9 +13,8 @@ def prepare_ocp(model, final_time, pw_max): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFes.declare_dynamics( - model, numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) - ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) @@ -32,7 +31,7 @@ def prepare_ocp(model, final_time, pw_max): return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/getting_started/optimization/pulse_width_optimization_nmpc.py b/examples/getting_started/optimization/pulse_width_optimization_nmpc.py index ffc3911c..c8a4f94c 100644 --- a/examples/getting_started/optimization/pulse_width_optimization_nmpc.py +++ b/examples/getting_started/optimization/pulse_width_optimization_nmpc.py @@ -50,7 +50,9 @@ def prepare_nmpc( total_cycle_len, total_cycle_duration ) - dynamics = OcpFes.declare_dynamics(model, numerical_data_time_series, ode_solver) + # dynamics = OcpFes.declare_dynamics(model, numerical_data_time_series, ode_solver) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=ode_solver) x_bounds = OcpFes.set_x_bounds(model) x_init_fes = OcpFes.set_x_init(model) @@ -94,7 +96,7 @@ def prepare_nmpc( return FesNmpc( bio_model=model, - dynamics=dynamics, + dynamics=dynamics_options, cycle_len=cycle_len, cycle_duration=cycle_duration, n_cycles_simultaneous=n_cycles_simultaneous, @@ -138,7 +140,8 @@ def main(plot=True): minimize_force=True, minimize_fatigue=False, use_sx=False, - ode_solver=OdeSolver.COLLOCATION(polynomial_degree=5, method="radau"), + ode_solver=OdeSolver.RK4(n_integration_steps=10), + # ode_solver=OdeSolver.COLLOCATION(polynomial_degree=5, method="radau"), ) def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): diff --git a/examples/identification/force_model/ding2003_model_id.py b/examples/identification/force_model/ding2003_model_id.py index 285ce005..676ea805 100644 --- a/examples/identification/force_model/ding2003_model_id.py +++ b/examples/identification/force_model/ding2003_model_id.py @@ -26,6 +26,7 @@ OcpFesId, ModelMaker, FES_plot, + OcpFes, ) from cocofest.identification.identification_method import DataExtraction @@ -66,11 +67,8 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFesId.declare_dynamics( - model=model, - numerical_data_timeseries=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10), - ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, @@ -97,7 +95,7 @@ def prepare_ocp( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, x_init=x_init, diff --git a/examples/identification/force_model/ding2007_model_id.py b/examples/identification/force_model/ding2007_model_id.py index d5144c91..b681c1b9 100644 --- a/examples/identification/force_model/ding2007_model_id.py +++ b/examples/identification/force_model/ding2007_model_id.py @@ -4,10 +4,9 @@ Finally, we use the data to identify the model parameters. """ -import matplotlib.pyplot as plt import numpy as np -from bioptim import SolutionMerge, OdeSolver, OptimalControlProgram, ObjectiveFcn, Node, ControlType, ObjectiveList +from bioptim import OdeSolver, OptimalControlProgram, ObjectiveFcn, Node, ControlType, ObjectiveList from cocofest import ( DingModelPulseWidthFrequency, @@ -15,6 +14,7 @@ ModelMaker, OcpFesId, FES_plot, + OcpFes, ) from cocofest.identification.identification_method import DataExtraction @@ -56,11 +56,8 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFesId.declare_dynamics( - model=model, - numerical_data_timeseries=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10), - ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, @@ -92,7 +89,7 @@ def prepare_ocp( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, x_init=x_init, diff --git a/examples/identification/force_model/hmed2018_model_id.py b/examples/identification/force_model/hmed2018_model_id.py index 3e247556..f0ca0062 100644 --- a/examples/identification/force_model/hmed2018_model_id.py +++ b/examples/identification/force_model/hmed2018_model_id.py @@ -1,6 +1,5 @@ import numpy as np -import matplotlib.pyplot as plt -from bioptim import OdeSolver, ObjectiveList, ObjectiveFcn, Node, OptimalControlProgram, ControlType, SolutionMerge +from bioptim import OdeSolver, ObjectiveList, ObjectiveFcn, Node, OptimalControlProgram, ControlType from cocofest import ( ModelMaker, @@ -8,6 +7,7 @@ IvpFes, OcpFesId, FES_plot, + OcpFes, ) from cocofest.identification.identification_method import DataExtraction @@ -50,11 +50,8 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFesId.declare_dynamics( - model=model, - numerical_data_timeseries=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10), - ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, @@ -86,7 +83,7 @@ def prepare_ocp( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, x_init=x_init, @@ -138,7 +135,7 @@ def main(plot=True): default_model = DingModelPulseIntensityFrequency() FES_plot(data=sol).plot( - title="Identification of HMed 2018 parameters", + title="Identification of Hmed 2018 parameters", tracked_data=sim_data, default_model=default_model, show_bounds=False, diff --git a/examples/other_fes_models/marion2009_example.py b/examples/other_fes_models/marion2009_example.py index 53d5ebeb..8f383515 100644 --- a/examples/other_fes_models/marion2009_example.py +++ b/examples/other_fes_models/marion2009_example.py @@ -9,6 +9,7 @@ Solver, ObjectiveList, ObjectiveFcn, + OdeSolver, OptimalControlProgram, ControlType, Node, @@ -23,7 +24,8 @@ def prepare_ocp(model, final_time, pw_max=0.0006): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFes.declare_dynamics(model, numerical_data_time_series) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) @@ -44,7 +46,7 @@ def prepare_ocp(model, final_time, pw_max=0.0006): return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/other_fes_models/marion2013_example.py b/examples/other_fes_models/marion2013_example.py index f5afc01c..99d8c667 100644 --- a/examples/other_fes_models/marion2013_example.py +++ b/examples/other_fes_models/marion2013_example.py @@ -11,6 +11,7 @@ Solver, ObjectiveList, ObjectiveFcn, + OdeSolver, OptimalControlProgram, ControlType, Node, @@ -26,7 +27,8 @@ def prepare_ocp(model, final_time, pw_max=0.0006): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFes.declare_dynamics(model, numerical_data_time_series) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) @@ -59,7 +61,7 @@ def prepare_ocp(model, final_time, pw_max=0.0006): return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/other_fes_models/veltink1992_example.py b/examples/other_fes_models/veltink1992_example.py index aed6a0cb..11b71923 100644 --- a/examples/other_fes_models/veltink1992_example.py +++ b/examples/other_fes_models/veltink1992_example.py @@ -1,12 +1,12 @@ from cocofest import ( VeltinkModelPulseIntensity, VeltinkRienerModelPulseIntensityWithFatigue, + OcpFes ) from bioptim import ( OptimalControlProgram, - PhaseDynamics, - DynamicsList, + OdeSolver, Solver, ObjectiveList, ObjectiveFcn, @@ -17,14 +17,8 @@ def prepare_ocp(model, final_time, n_shooting, fmax): # --- Set dynamics --- # - dynamics = DynamicsList() - dynamics.add( - model.declare_model_variables, - dynamic_function=model.dynamics, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=None, - ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=None, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) # --- Set initial guesses and bounds for states and controls --- # x_bounds = BoundsList() @@ -42,7 +36,7 @@ def prepare_ocp(model, final_time, n_shooting, fmax): return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, objective_functions=objective_functions, diff --git a/examples/sensitivity/force_length_velocity/muscle_force_length_and_force_velocity_relationships_comparison.py b/examples/sensitivity/force_length_velocity/muscle_force_length_and_force_velocity_relationships_comparison.py deleted file mode 100644 index dbcf8940..00000000 --- a/examples/sensitivity/force_length_velocity/muscle_force_length_and_force_velocity_relationships_comparison.py +++ /dev/null @@ -1,65 +0,0 @@ -""" -This example is used to compare the effect of the muscle force-length and force-velocity relationships -on the joint angle. -""" - -import numpy as np - -import matplotlib.pyplot as plt - -from bioptim import SolutionMerge - -from cocofest import DingModelPulseWidthFrequencyWithFatigue, OcpFesMsk, FesMskModel - -minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 - -sol_list = [] -sol_time = [] -activate_force_length_relationship = [False, True] - -for i in range(2): - - model = FesMskModel( - name=None, - biorbd_path="../../msk_models/Arm26/arm26_biceps_1dof.bioMod", - muscles_model=[DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIClong")], - activate_force_length_relationship=activate_force_length_relationship[i], - activate_force_velocity_relationship=activate_force_length_relationship[i], - activate_residual_torque=False, - ) - - ocp = OcpFesMsk.prepare_ocp( - model=model, - stim_time=np.linspace(0, 1, 11)[:-1], - final_time=1, - pulse_width={"fixed": 0.00025}, - msk_info={ - "bound_type": "start", - "bound_data": [0], - "with_residual_torque": False, - }, - use_sx=False, - ) - sol = ocp.solve() - sol_list.append(sol.decision_states(to_merge=[SolutionMerge.NODES, SolutionMerge.PHASES])) - time = np.concatenate( - sol.stepwise_time(to_merge=[SolutionMerge.NODES, SolutionMerge.PHASES], duplicated_times=False), - axis=0, - ) - index = 0 - for j in range(len(sol.ocp.nlp) - 1): - index = index + 1 + sol.ocp.nlp[j].ns - time = np.insert(time, index, time[index - 1]) - - sol_time.append(time) - -plt.plot(sol_time[0], np.degrees(sol_list[0]["q"][0]), label="without relationships") -plt.plot(sol_time[1], np.degrees(sol_list[1]["q"][0]), label="with relationships") - -plt.xlabel("Time (s)") -plt.ylabel("Angle (°)") -plt.legend() -plt.show() - -joint_overestimation = np.degrees(sol_list[0]["q"][0][-1]) - np.degrees(sol_list[1]["q"][0][-1]) -print(f"Joint overestimation: {joint_overestimation} degrees") diff --git a/examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py b/examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py new file mode 100644 index 00000000..bb111967 --- /dev/null +++ b/examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py @@ -0,0 +1,128 @@ +""" +This example is used to compare the effect of the muscle force-length, force-velocity, and passive-force relationships. +""" + +import numpy as np + +import matplotlib.pyplot as plt + +from bioptim import (SolutionMerge, + OdeSolver, + ObjectiveList, + ObjectiveFcn, + ParameterList, + OptimalControlProgram, + ControlType, + ConstraintFcn, + ConstraintList, + Node, + ) + +from cocofest import DingModelPulseWidthFrequencyWithFatigue, OcpFesMsk, FesMskModel, CustomObjective, OcpFes + + +def prepare_ocp(model: FesMskModel, final_time: float, msk_info: dict, fixed_pw): + + muscle_model = model.muscles_dynamics_model[0] + n_shooting = muscle_model.get_n_shooting(final_time) + numerical_data_time_series, stim_idx_at_node_list = muscle_model.get_numerical_data_time_series( + n_shooting, final_time + ) + dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10)) + + x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) + u_bounds, u_init = OcpFesMsk.set_u_bounds(model, msk_info["with_residual_torque"], max_bound=0.0006) + + objective_functions = ObjectiveList() + objective_functions.add( + CustomObjective.minimize_overall_muscle_force_production, + custom_type=ObjectiveFcn.Lagrange, + weight=1, + quadratic=True, + ) + + model = OcpFesMsk.update_model(model, parameters=ParameterList(use_sx=True), + external_force_set=None) + + constraints = ConstraintList() + constraints.add( + ConstraintFcn.BOUND_CONTROL, + key="last_pulse_width_BIClong", + node=Node.ALL_SHOOTING, + min_bound=np.array([fixed_pw]), + max_bound=np.array([fixed_pw]), + ) + + return OptimalControlProgram( + bio_model=[model], + dynamics=dynamics_options, + n_shooting=n_shooting, + phase_time=final_time, + objective_functions=objective_functions, + x_init=x_init, + x_bounds=x_bounds, + u_init=u_init, + u_bounds=u_bounds, + constraints=constraints, + control_type=ControlType.CONSTANT, + use_sx=True, + n_threads=20, + ) + + +# --- Main --- # +minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 + +sol_list = [] +sol_time = [] + +relationship_activation_dict = {"no_relationship": [False, False, False], + "length": [True, False, False], + "velocity":[False, True, False], + "passive_force":[False, False, True], + "length_velocity": [True, True, False], + "length_passive_force": [True, False, True], + "velocity_passive_force":[False, True, True], + "all_relationship": [True, True, True]} + +keys = list(relationship_activation_dict.keys()) +for key in keys: + stim_time = np.linspace(0, 1, 11)[:-1] + model = FesMskModel( + name=None, + biorbd_path="../../msk_models/Arm26/arm26_biceps_1dof.bioMod", + muscles_model=[DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIClong")], + stim_time=list(stim_time), + activate_force_length_relationship=relationship_activation_dict[key][0], + activate_force_velocity_relationship=relationship_activation_dict[key][1], + activate_passive_force_relationship=relationship_activation_dict[key][2], + activate_residual_torque=False, + ) + + ocp = prepare_ocp( + model=model, + final_time=1, + msk_info={ + "bound_type": "start", + "bound_data": [0], + "with_residual_torque": False, + }, + fixed_pw=0.00025 + ) + + sol = ocp.solve() + sol_list.append(sol.stepwise_states(to_merge=[SolutionMerge.NODES, SolutionMerge.PHASES])) + sol_time.append(sol.stepwise_time(to_merge=[SolutionMerge.NODES, SolutionMerge.PHASES])) + +for i in range(len(sol_time)): + key = keys[i] + plt.plot(sol_time[i], np.degrees(sol_list[i]["q"][0]), label=key) + + joint_error = np.degrees(sol_list[-1]["q"][0][-1]) - np.degrees(sol_list[i]["q"][0][-1]) + print(f"Joint error for {key}: {joint_error} degrees") + +plt.xlabel("Time (s)") +plt.ylabel("Angle (°)") +plt.legend() +plt.show() diff --git a/external/bioptim b/external/bioptim index d0d8d4b7..ec85e049 160000 --- a/external/bioptim +++ b/external/bioptim @@ -1 +1 @@ -Subproject commit d0d8d4b7fb8e0fc67584ec53c1ae8dfeac02bba7 +Subproject commit ec85e0497d49cf21f1d79168a70014b039b45802 diff --git a/tests/shard1/test_models_dynamics_without_bioptim.py b/tests/shard1/test_models_dynamics_without_bioptim.py index 4de8d2f8..f7bf376b 100644 --- a/tests/shard1/test_models_dynamics_without_bioptim.py +++ b/tests/shard1/test_models_dynamics_without_bioptim.py @@ -47,13 +47,10 @@ def test_ding2003_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - cn=5, - f=100, - a=3009, - tau1=0.050957, - km=0.103, - t=0.11, - t_stim_prev=np.array([0, 0.1]), + time=0.11, + states=[5, 100, 3009, 0.050957, 0.103], + controls=None, + numerical_timeseries=np.array([0, 0.1]), ) ).squeeze(), np.array(DM([-219.4399, 2037.0703, -40, 0.0021, 0.0019])).squeeze(), @@ -126,14 +123,10 @@ def test_ding2007_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - cn=5, - f=100, - a=4920, - tau1=0.050957, - km=0.103, - t=0.11, - t_stim_prev=np.array([0, 0.1]), - pulse_width=0.0002, + time=0.11, + states=[5, 100, 4920, 0.050957, 0.103], + controls=[0.0002], + numerical_timeseries=np.array([0, 0.1]), ) ).squeeze(), np.array(DM([-4.179e02, -4.905e02, -40.0, 2.1759e-03, 2.1677e-03])).squeeze(), @@ -215,14 +208,10 @@ def test_hmed2018_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - cn=5, - f=100, - a=3009, - tau1=0.050957, - km=0.103, - t=0.11, - t_stim_prev=np.array([0, 0.1]), - pulse_intensity=[30, 50], + time=0.11, + states=[5, 100, 3009, 0.050957, 0.103], + controls=[30, 50], + numerical_timeseries=np.array([0, 0.1]), ) ).squeeze(), np.array(DM([-241, 2037.07, -40, 0.0021, 1.9e-03])).squeeze(), @@ -289,9 +278,10 @@ def test_veltink1992_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - a=0.5, - mu=0.1, - I=50, + time=None, + states=[0.5, 0.1], + controls=[50], + numerical_timeseries=None, ) ).squeeze(), np.array(DM([0.96153846, 0.01066667])).squeeze(), @@ -358,14 +348,10 @@ def test_marion2009_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - cn=5, - f=100, - a=1473, - tau1=0.04298, - km=0.128, - t=0.11, - t_stim_prev=np.array([0, 0.1]), - theta=20, + time=0.11, + states=[5, 100, 1473, 0.04298, 0.128], + controls=np.array([20]), + numerical_timeseries=np.array([0, 0.1]), ) ).squeeze(), np.array(DM([-2.19408644e02, 1.04853099e03, -2.32015336e01, 1.56300000e-02, 6.26900000e-03])).squeeze(), @@ -432,15 +418,10 @@ def test_marion2009_modified_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - cn=5, - f=100, - a=1473, - tau1=0.04298, - km=0.128, - t=0.11, - t_stim_prev=np.array([0, 0.1]), - pulse_width=0.0004, - theta=20, + time=0.11, + states=[5, 100, 1473, 0.04298, 0.128], + controls=np.array([0.0004, 20]), + numerical_timeseries=np.array([0, 0.1]), ) ).squeeze(), np.array(DM([-2.19408644e02, 6.13622448e02, -2.00600000e01, 1.56300000e-02, 6.26900000e-03])).squeeze(), @@ -517,16 +498,10 @@ def test_marion2013_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - cn=5, - f=100, - theta=90, - dtheta_dt=0, - a=2100, - tau1=0.0361, - km=0.352, - t=0.11, - t_stim_prev=np.array([0, 0.1]), - Fload=0, + time=0.11, + states=[5, 100, 90, 0, 2100, 0.0361, 0.352], + controls=np.array([0]), + numerical_timeseries=np.array([0, 0.1]), ) ).squeeze(), np.array( @@ -619,17 +594,10 @@ def test_marion2013_modified_dynamics(): np.testing.assert_almost_equal( np.array( model.system_dynamics( - cn=5, - f=100, - theta=90, - dtheta_dt=0, - a=2100, - tau1=0.0361, - km=0.352, - t=0.11, - t_stim_prev=np.array([0, 0.1]), - pulse_width=0.0004, - Fload=0, + time=0.11, + states=[5, 100, 90, 0, 2100, 0.0361, 0.352], + controls=np.array([0.0004, 0]), + numerical_timeseries=np.array([0, 0.1]), ) ).squeeze(), np.array( diff --git a/tests/shard1/test_ocp_id.py b/tests/shard1/test_ocp_id.py index 65e8edbc..4185f36a 100644 --- a/tests/shard1/test_ocp_id.py +++ b/tests/shard1/test_ocp_id.py @@ -69,9 +69,8 @@ def prepare_ocp_ding2003( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFesId.declare_dynamics( - model=model, - numerical_data_timeseries=numerical_data_time_series, + dynamics_options = OcpFesId.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10), ) @@ -100,7 +99,7 @@ def prepare_ocp_ding2003( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, x_init=x_init, @@ -238,9 +237,8 @@ def prepare_ocp_ding2007( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFesId.declare_dynamics( - model=model, - numerical_data_timeseries=numerical_data_time_series, + dynamics_options = OcpFesId.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10), ) @@ -274,7 +272,7 @@ def prepare_ocp_ding2007( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, x_init=x_init, @@ -422,9 +420,8 @@ def prepare_ocp_hmed2018( numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics = OcpFesId.declare_dynamics( - model=model, - numerical_data_timeseries=numerical_data_time_series, + dynamics_options = OcpFesId.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10), ) @@ -458,7 +455,7 @@ def prepare_ocp_hmed2018( return OptimalControlProgram( bio_model=[model], - dynamics=dynamics, + dynamics=dynamics_options, n_shooting=n_shooting, phase_time=final_time, x_init=x_init, diff --git a/tests/shard2/test_run_examples.py b/tests/shard2/test_run_examples.py index 2fb22924..989fff1e 100644 --- a/tests/shard2/test_run_examples.py +++ b/tests/shard2/test_run_examples.py @@ -74,15 +74,15 @@ def test_multibody_examples(module_name): cycling_biorbd_model_path = biomodel_folder + "/Wu/Modified_Wu_Shoulder_Model_Cycling.bioMod" cycling_initial_guess_biorbd_model_path = biomodel_folder + "/Wu/Modified_Wu_Shoulder_Model_Cycling_for_IK.bioMod" - -@pytest.mark.parametrize("module_name", MULTIBODY_CYCLING_EXAMPLE_MODULES) -def test_cycling_multibody_examples(module_name): - ocp_module = importlib.import_module(module_name) - ocp_module.main( - plot=False, - model_path=cycling_biorbd_model_path, - initial_guess_model_path=cycling_initial_guess_biorbd_model_path, - ) +# TODO: Update it +# @pytest.mark.parametrize("module_name", MULTIBODY_CYCLING_EXAMPLE_MODULES) +# def test_cycling_multibody_examples(module_name): +# ocp_module = importlib.import_module(module_name) +# ocp_module.main( +# plot=False, +# model_path=cycling_biorbd_model_path, +# initial_guess_model_path=cycling_initial_guess_biorbd_model_path, +# ) MULTIBODY_FLEXION_EXAMPLE_MODULES = [ From 93268898b896ef0f172553a3aee28d8dbe8d5223 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 21 Nov 2025 13:38:30 -0500 Subject: [PATCH 2/5] nmpc, collocation and contact point update --- cocofest/integration/ivp_fes.py | 4 +- cocofest/models/ding2003/ding2003.py | 75 +++++----- .../models/ding2003/ding2003_with_fatigue.py | 12 +- cocofest/models/ding2007/ding2007.py | 14 +- .../models/ding2007/ding2007_with_fatigue.py | 4 +- cocofest/models/dynamical_model.py | 71 +++++++--- cocofest/models/fes_model.py | 20 ++- cocofest/models/hmed2018/hmed2018.py | 17 +-- .../models/hmed2018/hmed2018_with_fatigue.py | 4 +- .../marion2009_modified_with_fatigue.py | 4 +- .../marion2009/marion2009_with_fatigue.py | 4 +- cocofest/models/marion2013/marion2013.py | 4 +- .../models/marion2013/marion2013_modified.py | 4 +- .../marion2013_modified_with_fatigue.py | 4 +- .../marion2013/marion2013_with_fatigue.py | 4 +- cocofest/models/state_configure.py | 22 +-- cocofest/models/veltink1992/veltink1992.py | 88 ++++++++++-- .../veltink1992/veltink1992_and_riener1998.py | 4 +- cocofest/optimization/fes_id_ocp.py | 2 +- cocofest/optimization/fes_ocp.py | 4 +- cocofest/optimization/fes_ocp_multibody.py | 2 +- .../cycling/cycling_pulse_width_mhe.py | 6 +- .../cycling_with_different_driven_methods.py | 128 ++++++++++-------- external/bioptim | 2 +- .../test_models_dynamics_without_bioptim.py | 16 +-- tests/shard2/test_run_examples.py | 18 +-- 26 files changed, 317 insertions(+), 220 deletions(-) diff --git a/cocofest/integration/ivp_fes.py b/cocofest/integration/ivp_fes.py index b081a81f..c858202b 100644 --- a/cocofest/integration/ivp_fes.py +++ b/cocofest/integration/ivp_fes.py @@ -326,9 +326,7 @@ def build_initial_guess_from_ocp(self, ocp, stim_idx_at_node_list=None): p = InitialGuessList() s = InitialGuessList() - muscle_name = "_" + ocp.model.muscle_name if ocp.model.muscle_name else "" - for j in range(len(self.model.name_dof)): - key = ocp.model.name_dof[j] + muscle_name + for j, key in enumerate(self.model.name_dofs): x.add(key=key, initial_guess=ocp.model.standard_rest_values()[j], phase=0) if ocp.controls_keys: diff --git a/cocofest/models/ding2003/ding2003.py b/cocofest/models/ding2003/ding2003.py index 36d43f3d..3edde612 100644 --- a/cocofest/models/ding2003/ding2003.py +++ b/cocofest/models/ding2003/ding2003.py @@ -1,4 +1,4 @@ -from typing import Callable +from typing import Callable, List from math import gcd from fractions import Fraction @@ -9,14 +9,16 @@ DynamicsEvaluation, NonLinearProgram, StateDynamics, - FcnEnum, + DynamicsFunctions, + OdeSolver, + States, ) from cocofest.models.state_configure import StateConfigure from cocofest.models.fes_model import FesModel -class DingModelFrequency(StateDynamics, FesModel): +class DingModelFrequency(FesModel, StateDynamics): """ This is a custom model of the Bioptim package. As CustomModel, some methods are mandatory and must be implemented. to make it work with bioptim. @@ -38,8 +40,9 @@ def __init__( stim_time: list[float] = None, previous_stim: dict = None, sum_stim_truncation: int = 20, + **kwargs ): - super().__init__() + super().__init__(name=model_name, **kwargs) self._model_name = model_name self._muscle_name = muscle_name self.sum_stim_truncation = sum_stim_truncation @@ -74,10 +77,25 @@ def __init__( self.force_velocity_relationship = 1 self.passive_force_relationship = 0 - # ---- Declare dynamic variable ---- # - self.state_configuration = [CustomStates.my_muscle_states] - self.contact_types = () + # --- Configure variables --- # + @property + def state_configuration_functions(self) -> List[States | Callable]: + return [StateConfigure().configure_all_muscle_states] + + @property + def control_configuration_functions(self) -> List[States | Callable]: + return [] + + @property + def algebraic_configuration_functions(self) -> List[States | Callable]: + return [] + + @property + def extra_configuration_functions(self) -> List[States | Callable]: + return [] + + # --- Set model parameters --- # def set_a_rest(self, model, a_rest: MX | float): # models is required for bioptim compatibility self.a_rest = a_rest @@ -116,8 +134,8 @@ def serialize(self) -> tuple[Callable, dict]: # ---- Needed for the example ---- # @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return ["Cn" + muscle_name, "F" + muscle_name] @property @@ -345,32 +363,22 @@ def dynamics( """ model = self.fes_model if self.fes_model else nlp.model dxdt_fun = model.system_dynamics + dxdt = dxdt_fun( + time=time, + states=states, + controls=controls, + numerical_timeseries=numerical_timeseries, + ) defects = None - # TODO: Get defects for fes model states - # 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 - - # if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): - # slope_states = vertcat(*[DynamicsFunctions.get(nlp.states_dot[key], nlp.states_dot.scaled.cx) for key in nlp.states.keys()]) - # dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - # total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau - # external_forces = nlp.get_external_forces("external_forces", states, controls, algebraic_states, - # numerical_data_timeseries) - # ddq = nlp.model.forward_dynamics(with_contact=self.with_contact)( - # q, qdot, total_torque, external_forces, parameters - # ) # q, qdot, tau, external_forces, parameters - # defects = slope_states * nlp.dt - vertcat(dq, ddq) * nlp.dt + if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): + states_dot_list = [] + for key in model.name_dofs: + states_dot_list.append(DynamicsFunctions.get(nlp.states_dot[key], nlp.states_dot.scaled.cx)) + defects = vertcat(*states_dot_list) - dxdt return DynamicsEvaluation( - dxdt=dxdt_fun( - time=time, - states=states, - controls=controls, - numerical_timeseries=numerical_timeseries, - ), + dxdt=dxdt, defects=defects, ) @@ -438,8 +446,3 @@ def get_n_shooting(self, final_time: float) -> int: ) return n_shooting - - -# TODO: Change to future bioptim version -class CustomStates(FcnEnum): - my_muscle_states = (StateConfigure().configure_all_muscle_states,) diff --git a/cocofest/models/ding2003/ding2003_with_fatigue.py b/cocofest/models/ding2003/ding2003_with_fatigue.py index c417b1e1..0ce2a51c 100644 --- a/cocofest/models/ding2003/ding2003_with_fatigue.py +++ b/cocofest/models/ding2003/ding2003_with_fatigue.py @@ -3,15 +3,7 @@ from casadi import MX, vertcat import numpy as np -from bioptim import ( - ConfigureProblem, - DynamicsEvaluation, - NonLinearProgram, - OptimalControlProgram, -) - from cocofest.models.ding2003.ding2003 import DingModelFrequency -from cocofest.models.state_configure import StateConfigure class DingModelFrequencyWithFatigue(DingModelFrequency): @@ -101,8 +93,8 @@ def serialize(self) -> tuple[Callable, dict]: # ---- Needed for the example ---- # @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/ding2007/ding2007.py b/cocofest/models/ding2007/ding2007.py index a7491f97..1e41a478 100644 --- a/cocofest/models/ding2007/ding2007.py +++ b/cocofest/models/ding2007/ding2007.py @@ -1,8 +1,8 @@ -from typing import Callable +from typing import Callable, List from casadi import MX, vertcat, exp -from bioptim import FcnEnum +from bioptim import States from cocofest.models.ding2003.ding2003 import DingModelFrequency from cocofest.models.state_configure import StateConfigure @@ -61,7 +61,9 @@ def __init__( self.tauc = TAUC_DEFAULT self.fmax = 248 # Maximum force (N) at 100 Hz and 600 us - self.control_configuration = [CustomStates.my_control] + @property + def control_configuration_functions(self) -> List[States | Callable]: + return [StateConfigure().configure_last_pulse_width] @property def identifiable_parameters(self): @@ -198,9 +200,3 @@ def set_impulse_width(self, value: list[MX]): The pulsation duration list (s) """ self.pulse_width = value - - -# TODO: Change to future bioptim version -class CustomStates(FcnEnum): - my_control = (StateConfigure().configure_last_pulse_width,) - diff --git a/cocofest/models/ding2007/ding2007_with_fatigue.py b/cocofest/models/ding2007/ding2007_with_fatigue.py index 13e75829..2d56f059 100644 --- a/cocofest/models/ding2007/ding2007_with_fatigue.py +++ b/cocofest/models/ding2007/ding2007_with_fatigue.py @@ -52,8 +52,8 @@ def __init__( # ---- Absolutely needed methods ---- # @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 1d6a6c80..235915ef 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -1,14 +1,14 @@ -from typing import Callable +from typing import Callable, List import numpy as np from casadi import vertcat, MX, SX, Function from bioptim import ( BiorbdModel, ConfigureVariables, + ContactType, DynamicsEvaluation, DynamicsFunctions, ExternalForceSetTimeSeries, - FcnEnum, NonLinearProgram, OdeSolver, OptimalControlProgram, @@ -28,7 +28,7 @@ muscle_passive_force_coefficient, ) -class FesMskModel(StateDynamics, BiorbdModel): +class FesMskModel(BiorbdModel, StateDynamics): def __init__( self, name: str = None, @@ -66,7 +66,11 @@ def __init__( parameters: ParameterList The parameters that will be used in the model """ - super().__init__(bio_model=biorbd_path, parameters=parameters, external_force_set=external_force_set) + self.with_contact = with_contact + super().__init__(bio_model=biorbd_path, + parameters=parameters, + external_force_set=external_force_set, + contact_types=[ContactType.RIGID_EXPLICIT] if self.with_contact else []) self._name = name self.biorbd_path = biorbd_path @@ -94,11 +98,46 @@ def __init__( self.parameters_list = parameters self.external_forces_set = external_force_set self.muscles_model = muscles_model - self.with_contact = with_contact - self.state_configuration = [CustomStates.my_muscles_states, States.Q, States.QDOT] - self.control_configuration = [CustomStates.my_controls] + @property + def state_configuration_functions(self) -> List[States | Callable]: + return [StateConfigure().configure_all_muscle_msk_states, States.Q, States.QDOT] + + @property + def control_configuration_functions(self) -> list: + return [FesMskModel.declare_model_control] + + @property + def algebraic_configuration_functions(self) -> list: + return [] + + @property + def extra_configuration_functions(self) -> list: + return [] + + @property + def contact_types(self) -> List[ContactType]: + return [ContactType.RIGID_EXPLICIT] if self.with_contact else [] + + def get_rigid_contact_forces( + self, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, + nlp, + ): + q = DynamicsFunctions.get(nlp.states["q"], states) + qdot = DynamicsFunctions.get(nlp.states["qdot"], states) + tau = DynamicsFunctions.get(nlp.controls["tau"], controls) if "tau" in nlp.controls.keys() else MX() + external_forces = nlp.get_external_forces( + "external_forces", states, controls, algebraic_states, numerical_timeseries + ) + + return nlp.model.rigid_contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) # ---- Absolutely needed methods ---- # def serialize(self) -> tuple[Callable, dict]: @@ -189,22 +228,18 @@ def dynamics( total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau external_forces = nlp.get_external_forces("external_forces", states, controls, algebraic_states, numerical_data_timeseries) - # with_contact = ( - # True if nlp.model.bio_model.contact_names != () else False - # ) # TODO: Remove once added in supplementary dynamics - ddq = nlp.model.forward_dynamics(with_contact=self.with_contact)( q, qdot, total_torque, external_forces, parameters ) # q, qdot, tau, external_forces, parameters dxdt = vertcat(dxdt_muscle_list, dq, ddq) - # TODO: Missing defect for FES states 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 + states_dot_list = [] + for key in nlp.states_dot.keys(): + states_dot_list.append(DynamicsFunctions.get(nlp.states_dot[key], nlp.states_dot.scaled.cx)) + defects = vertcat(*states_dot_list) * nlp.dt - dxdt * nlp.dt return DynamicsEvaluation(dxdt=dxdt, defects=defects) @@ -482,9 +517,3 @@ def _check_numerical_timeseries_format(numerical_timeseries: np.ndarray, n_shoot f"The numerical_data_timeseries should be of format dict[str, np.ndarray] " f"where the list is the number of shooting points of the phase " ) - - -# TODO: Change to future bioptim version -class CustomStates(FcnEnum): - my_muscles_states = (StateConfigure().configure_all_muscle_msk_states,) - my_controls = (FesMskModel.declare_model_control,) diff --git a/cocofest/models/fes_model.py b/cocofest/models/fes_model.py index 997117e8..268263eb 100644 --- a/cocofest/models/fes_model.py +++ b/cocofest/models/fes_model.py @@ -1,14 +1,19 @@ from abc import ABC, abstractmethod -import numpy as np from casadi import MX -from bioptim import NonLinearProgram, OptimalControlProgram +from bioptim import NonLinearProgram class FesModel(ABC): - def __init__(self): + def __init__(self, name, **kwargs): + super().__init__(**kwargs) self.stim_time = None self.previous_stim = None + self._name = name + + @property + def name(self): + return self._name @abstractmethod def set_a_rest(self, model, a_rest: MX | float): @@ -64,15 +69,6 @@ def serialize(self): """ - @abstractmethod - def name_dof(self): - """ - - Returns - ------- - - """ - @abstractmethod def nb_state(self): """ diff --git a/cocofest/models/hmed2018/hmed2018.py b/cocofest/models/hmed2018/hmed2018.py index a71a71ea..6116fe1b 100644 --- a/cocofest/models/hmed2018/hmed2018.py +++ b/cocofest/models/hmed2018/hmed2018.py @@ -1,14 +1,13 @@ -from typing import Callable +from typing import Callable, List from casadi import MX, vertcat, tanh import numpy as np -from bioptim import FcnEnum +from bioptim import States from cocofest.models.ding2003.ding2003 import DingModelFrequency from cocofest.models.state_configure import StateConfigure - class DingModelPulseIntensityFrequency(DingModelFrequency): """ This is a custom models that inherits from bioptim. CustomModel. @@ -58,7 +57,10 @@ def __init__( self.cr = CR_DEFAULT self.impulse_intensity = None - self.control_configuration = [CustomStates.my_control] + + @property + def control_configuration_functions(self) -> List[States | Callable]: + return [StateConfigure().configure_pulse_intensity] @property def identifiable_parameters(self): @@ -75,7 +77,7 @@ def identifiable_parameters(self): @property def pulse_intensity_name(self): - muscle_name = "_" + self.muscle_name if self.muscle_name else "" + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return "pulse_intensity" + muscle_name def set_ar(self, model, ar: MX | float): @@ -218,8 +220,3 @@ def _get_additional_previous_stim_time(self): self.previous_stim["time"].insert(0, -10000000) self.previous_stim["pulse_intensity"].insert(0, 50) return self.previous_stim - - -# TODO: Change to future bioptim version -class CustomStates(FcnEnum): - my_control = (StateConfigure().configure_pulse_intensity,) diff --git a/cocofest/models/hmed2018/hmed2018_with_fatigue.py b/cocofest/models/hmed2018/hmed2018_with_fatigue.py index 41bf4a05..d0b777b7 100644 --- a/cocofest/models/hmed2018/hmed2018_with_fatigue.py +++ b/cocofest/models/hmed2018/hmed2018_with_fatigue.py @@ -53,8 +53,8 @@ def __init__( # ---- Absolutely needed methods ---- # @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2009/marion2009_modified_with_fatigue.py b/cocofest/models/marion2009/marion2009_modified_with_fatigue.py index acf64336..6d188776 100644 --- a/cocofest/models/marion2009/marion2009_modified_with_fatigue.py +++ b/cocofest/models/marion2009/marion2009_modified_with_fatigue.py @@ -54,8 +54,8 @@ def __init__( self.alpha_km = ALPHA_KM_DEFAULT @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2009/marion2009_with_fatigue.py b/cocofest/models/marion2009/marion2009_with_fatigue.py index 431ce752..e67c35e1 100644 --- a/cocofest/models/marion2009/marion2009_with_fatigue.py +++ b/cocofest/models/marion2009/marion2009_with_fatigue.py @@ -51,8 +51,8 @@ def __init__( self.alpha_km = ALPHA_KM_DEFAULT @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013.py b/cocofest/models/marion2013/marion2013.py index 13095e4c..8c56d341 100644 --- a/cocofest/models/marion2013/marion2013.py +++ b/cocofest/models/marion2013/marion2013.py @@ -60,8 +60,8 @@ def __init__( self.FM = FM_DEFAULT @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013_modified.py b/cocofest/models/marion2013/marion2013_modified.py index 766217c7..7bb9615f 100644 --- a/cocofest/models/marion2013/marion2013_modified.py +++ b/cocofest/models/marion2013/marion2013_modified.py @@ -68,8 +68,8 @@ def __init__( self.FM = FM_DEFAULT @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013_modified_with_fatigue.py b/cocofest/models/marion2013/marion2013_modified_with_fatigue.py index 332c317c..46e8c71a 100644 --- a/cocofest/models/marion2013/marion2013_modified_with_fatigue.py +++ b/cocofest/models/marion2013/marion2013_modified_with_fatigue.py @@ -58,8 +58,8 @@ def __init__( self.beta_a = BETA_A_DEFAULT @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013_with_fatigue.py b/cocofest/models/marion2013/marion2013_with_fatigue.py index 0eed3a81..5e992204 100644 --- a/cocofest/models/marion2013/marion2013_with_fatigue.py +++ b/cocofest/models/marion2013/marion2013_with_fatigue.py @@ -55,8 +55,8 @@ def __init__( self.beta_a = BETA_A_DEFAULT @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/state_configure.py b/cocofest/models/state_configure.py index 2b491545..065863df 100644 --- a/cocofest/models/state_configure.py +++ b/cocofest/models/state_configure.py @@ -418,7 +418,7 @@ def configure_angular_velocity( ) @staticmethod - def configure_last_pulse_width(ocp, nlp, muscle_name: str = None, as_states=True, as_controls=False, as_algebraic_states=False): + def configure_last_pulse_width(ocp, nlp, muscle_name: str = None): """ Configure the last pulse width control @@ -437,7 +437,7 @@ def configure_last_pulse_width(ocp, nlp, muscle_name: str = None, as_states=True ) @staticmethod - def configure_pulse_intensity(ocp, nlp, muscle_name: str = None, truncation: int = None, as_states=True, as_controls=False, as_algebraic_states=False): + def configure_pulse_intensity(ocp, nlp, muscle_name: str = None, truncation: int = None): """ Configure the pulse intensity control for the Ding model @@ -458,7 +458,7 @@ def configure_pulse_intensity(ocp, nlp, muscle_name: str = None, truncation: int ) @staticmethod - def configure_intensity(ocp, nlp, as_states=False, as_controls=True, as_algebraic_states=False): + def configure_intensity(ocp, nlp): """ Configure the intensity control for the Veltink1992 model @@ -478,8 +478,8 @@ def configure_intensity(ocp, nlp, as_states=False, as_controls=True, as_algebrai ) @staticmethod - def configure_all_muscle_states(ocp, nlp, as_states=True, as_controls=False, as_algebraic_states=False): - for state_key in nlp.model.name_dof: + def configure_all_muscle_states(ocp, nlp): + for state_key in nlp.model.name_dofs: if state_key in StateConfigure().state_dictionary.keys(): StateConfigure().state_dictionary[state_key]( ocp=ocp, @@ -490,11 +490,13 @@ def configure_all_muscle_states(ocp, nlp, as_states=True, as_controls=False, as_ ) @staticmethod - def configure_all_muscle_msk_states(ocp, nlp, as_states=True, as_controls=False, as_algebraic_states=False): + def configure_all_muscle_msk_states(ocp, nlp): for muscle_dynamics_model in nlp.model.muscles_dynamics_model: - for state_key in muscle_dynamics_model.name_dof: - if state_key in StateConfigure().state_dictionary.keys(): - StateConfigure().state_dictionary[state_key]( + for state_key in muscle_dynamics_model.name_dofs: + separator = "_" + key = state_key.split(separator, 1)[0] + if key in StateConfigure().state_dictionary.keys(): + StateConfigure().state_dictionary[key]( ocp=ocp, nlp=nlp, as_states=True, @@ -503,7 +505,7 @@ def configure_all_muscle_msk_states(ocp, nlp, as_states=True, as_controls=False, ) def configure_all_fes_model_states(self, ocp, nlp, fes_model): - for state_key in fes_model.name_dof: + for state_key in fes_model.name_dofs: if state_key in self.state_dictionary.keys(): self.state_dictionary[state_key]( ocp=ocp, diff --git a/cocofest/models/veltink1992/veltink1992.py b/cocofest/models/veltink1992/veltink1992.py index 0dc66979..dc10f2f2 100644 --- a/cocofest/models/veltink1992/veltink1992.py +++ b/cocofest/models/veltink1992/veltink1992.py @@ -1,10 +1,11 @@ -from typing import Callable +from typing import Callable, List from casadi import MX, vertcat import numpy as np -from bioptim import StateDynamics, FcnEnum +from bioptim import StateDynamics, States, OdeSolver, DynamicsEvaluation, NonLinearProgram, DynamicsFunctions from cocofest.models.state_configure import StateConfigure +from cocofest.models.fes_model import FesModel class VeltinkModelPulseIntensity(StateDynamics): @@ -40,12 +41,32 @@ def __init__( self.I_saturation = I_saturation if I_saturation is not None else I_SATURATION_DEFAULT self.contact_types = () - self.state_configuration = [CustomStates.my_muscle_states] - self.control_configuration = [CustomStates.my_control] + + + @property + def name(self): + return self._name + + # --- Configure variables --- # + @property + def state_configuration_functions(self) -> List[States | Callable]: + return [StateConfigure().configure_all_muscle_states] + + @property + def control_configuration_functions(self) -> List[States | Callable]: + return [StateConfigure().configure_intensity] + + @property + def algebraic_configuration_functions(self) -> List[States | Callable]: + return [] + + @property + def extra_configuration_functions(self) -> List[States | Callable]: + return [] @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return ["a" + muscle_name] # Only muscle activation state @property @@ -157,8 +178,55 @@ def system_dynamics( return vertcat(a_dot) + def dynamics( + self, + time: MX, + states: MX, + controls: MX, + parameters: MX, + algebraic_states: MX, + numerical_timeseries: MX, + nlp: NonLinearProgram, + ) -> DynamicsEvaluation: + """ + Functional electrical stimulation dynamic + + Parameters + ---------- + time: MX + The system's current node time + states: MX + The state of the system CN, F, A, Tau1, Km + controls: MX + The controls of the system, none + parameters: MX + The parameters acting on the system, final time of each phase + algebraic_states: MX + The stochastic variables of the system, none + numerical_timeseries: MX + The numerical timeseries of the system + nlp: NonLinearProgram + A reference to the phase + Returns + ------- + The derivative of the states in the tuple[MX] format + """ + dxdt_fun = nlp.model.system_dynamics + dxdt = dxdt_fun( + time=time, + states=states, + controls=controls, + numerical_timeseries=numerical_timeseries, + ) + + defects = None + if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): + states_dot_list = [] + for key in nlp.model.name_dofs: + states_dot_list.append(DynamicsFunctions.get(nlp.states_dot[key], nlp.states_dot.scaled.cx)) + defects = vertcat(*states_dot_list) - dxdt -# TODO: Change to future bioptim version -class CustomStates(FcnEnum): - my_muscle_states = (StateConfigure().configure_all_muscle_states,) - my_control = (StateConfigure().configure_intensity,) + return DynamicsEvaluation( + dxdt=dxdt, + defects=defects, + ) diff --git a/cocofest/models/veltink1992/veltink1992_and_riener1998.py b/cocofest/models/veltink1992/veltink1992_and_riener1998.py index 3bbe44bd..2bbcef4c 100644 --- a/cocofest/models/veltink1992/veltink1992_and_riener1998.py +++ b/cocofest/models/veltink1992/veltink1992_and_riener1998.py @@ -49,8 +49,8 @@ def __init__( self.T_rec = T_rec if T_rec is not None else T_REC_DEFAULT @property - def name_dof(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = "_" + self.muscle_name if self.muscle_name and with_muscle_name else "" + def name_dofs(self, with_muscle_name: bool = False) -> list[str]: + muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") return [ "a" + muscle_name, # Muscle activation "mu" + muscle_name, # Fatigue state diff --git a/cocofest/optimization/fes_id_ocp.py b/cocofest/optimization/fes_id_ocp.py index 8216c8a2..29ceed1c 100644 --- a/cocofest/optimization/fes_id_ocp.py +++ b/cocofest/optimization/fes_id_ocp.py @@ -37,7 +37,7 @@ def set_x_bounds( # Sets the bound for all the phases x_bounds = BoundsList() - variable_bound_list = model.name_dof + variable_bound_list = model.name_dofs starting_bounds, min_bounds, max_bounds = ( model.standard_rest_values(), model.standard_rest_values(), diff --git a/cocofest/optimization/fes_ocp.py b/cocofest/optimization/fes_ocp.py index 1c43dd1a..7e2b6e1f 100644 --- a/cocofest/optimization/fes_ocp.py +++ b/cocofest/optimization/fes_ocp.py @@ -100,7 +100,7 @@ def set_x_bounds(model): # Sets the bound for all the phases x_bounds = BoundsList() - variable_bound_list = model.name_dof + variable_bound_list = model.name_dofs starting_bounds, min_bounds, max_bounds = ( model.standard_rest_values(), model.standard_rest_values(), @@ -132,7 +132,7 @@ def set_x_bounds(model): @staticmethod def set_x_init(model): - variable_bound_list = model.name_dof + variable_bound_list = model.name_dofs x_init = InitialGuessList() for j in range(len(variable_bound_list)): x_init.add(variable_bound_list[j], model.standard_rest_values()[j]) diff --git a/cocofest/optimization/fes_ocp_multibody.py b/cocofest/optimization/fes_ocp_multibody.py index ba8d7e7b..e0d79ad9 100644 --- a/cocofest/optimization/fes_ocp_multibody.py +++ b/cocofest/optimization/fes_ocp_multibody.py @@ -109,7 +109,7 @@ def set_x_bounds_fes(bio_models): x_init = InitialGuessList() for model in bio_models.muscles_dynamics_model: muscle_name = model.muscle_name - variable_bound_list = [model.name_dof[i] + "_" + muscle_name for i in range(len(model.name_dof))] + variable_bound_list = model.name_dofs starting_bounds, min_bounds, max_bounds = ( model.standard_rest_values(), diff --git a/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py b/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py index d727d438..8653993e 100644 --- a/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py +++ b/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py @@ -653,6 +653,7 @@ def updating_model(model: FesMskModel, external_force_set, parameters=None) -> F activate_residual_torque=model.activate_residual_torque, parameters=parameters, external_force_set=external_force_set, + with_contact=True, ) return model @@ -716,6 +717,7 @@ def set_fes_model(model_path, stim_time): activate_passive_force_relationship=True, activate_residual_torque=False, external_force_set=None, # External forces will be added later (resistive_torque) + with_contact=True, ) return fes_model @@ -1011,9 +1013,9 @@ def main( main( stimulation_frequency=30, - n_total_cycle=1, + n_total_cycle=5, n_cycles_simultaneous=[2], # [2, 3, 4, 5] - resistive_torque=-0.3, # (N.m) + resistive_torque=-0.2, # (N.m) cost_fun_weight=[(1, 0, 0)], # (min_force, min_fatigue, min_control) init_guess=False, save=False, diff --git a/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py b/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py index 53ec3e5f..a3ed11e2 100644 --- a/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py +++ b/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py @@ -26,7 +26,8 @@ PhaseDynamics, Solver, VariableScalingList, - ContactType, + DynamicsOptionsList, + DynamicsOptions, ) from cocofest import ( @@ -90,9 +91,11 @@ def update_model( previous_stim=model.muscles_dynamics_model[0].previous_stim, activate_force_length_relationship=model.activate_force_length_relationship, activate_force_velocity_relationship=model.activate_force_velocity_relationship, + activate_passive_force_relationship=model.activate_force_velocity_relationship, activate_residual_torque=model.activate_residual_torque, parameters=parameters, external_force_set=external_force_set, + with_contact=model.with_contact ) else: model = BiorbdModel(model.path, external_force_set=external_force_set) @@ -100,55 +103,55 @@ def update_model( return model -def set_dynamics( - model: BiorbdModel | FesMskModel, - numerical_time_series: dict, - dynamics_type_str: str = "torque_driven", - ode_solver: OdeSolver = OdeSolver.RK4(n_integration_steps=10), -) -> DynamicsList: - """ - Set the dynamics of the optimal control program based on the chosen dynamics type. - - Parameters - ---------- - model: BiorbdModel | FesMskModel - The biomechanical model. - numerical_time_series: dict - External numerical data (e.g., external forces). - dynamics_type_str: str - Type of dynamics ("torque_driven", "muscle_driven", or "fes_driven"). - - Returns - ------- - A DynamicsList configured for the problem. - """ - dynamics_type = ( - DynamicsFcn.TORQUE_DRIVEN - if dynamics_type_str == "torque_driven" - else ( - DynamicsFcn.MUSCLE_DRIVEN - if dynamics_type_str == "muscle_driven" - else model.declare_model_variables if dynamics_type_str == "fes_driven" else None - ) - ) - if dynamics_type is None: - raise ValueError("Dynamics type not recognized") - - dynamics = DynamicsList() - dynamics.add( - dynamics_type=dynamics_type, - dynamic_function=( - None if dynamics_type in (DynamicsFcn.TORQUE_DRIVEN, DynamicsFcn.MUSCLE_DRIVEN) else model.muscle_dynamic - ), - expand_dynamics=True, - expand_continuity=False, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - phase=0, - ode_solver=ode_solver, - contact_type=[ContactType.RIGID_EXPLICIT], - ) - return dynamics +# def set_dynamics( +# model: BiorbdModel | FesMskModel, +# numerical_time_series: dict, +# dynamics_type_str: str = "torque_driven", +# ode_solver: OdeSolver = OdeSolver.RK4(n_integration_steps=10), +# ) -> DynamicsList: +# """ +# Set the dynamics of the optimal control program based on the chosen dynamics type. +# +# Parameters +# ---------- +# model: BiorbdModel | FesMskModel +# The biomechanical model. +# numerical_time_series: dict +# External numerical data (e.g., external forces). +# dynamics_type_str: str +# Type of dynamics ("torque_driven", "muscle_driven", or "fes_driven"). +# +# Returns +# ------- +# A DynamicsList configured for the problem. +# """ +# dynamics_type = ( +# DynamicsFcn.TORQUE_DRIVEN +# if dynamics_type_str == "torque_driven" +# else ( +# DynamicsFcn.MUSCLE_DRIVEN +# if dynamics_type_str == "muscle_driven" +# else model.declare_model_variables if dynamics_type_str == "fes_driven" else None +# ) +# ) +# if dynamics_type is None: +# raise ValueError("Dynamics type not recognized") +# +# dynamics = DynamicsList() +# dynamics.add( +# dynamics_type=dynamics_type, +# dynamic_function=( +# None if dynamics_type in (DynamicsFcn.TORQUE_DRIVEN, DynamicsFcn.MUSCLE_DRIVEN) else model.muscle_dynamic +# ), +# expand_dynamics=True, +# expand_continuity=False, +# phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, +# numerical_data_timeseries=numerical_time_series, +# phase=0, +# ode_solver=ode_solver, +# contact_type=[ContactType.RIGID_EXPLICIT], +# ) +# return dynamics def set_objective_functions(model: BiorbdModel | FesMskModel, dynamics_type: str, init_x) -> ObjectiveList: @@ -450,11 +453,21 @@ def prepare_ocp( numerical_time_series.update(numerical_data_time_series) # Set dynamics based on the chosen dynamics type - dynamics = set_dynamics( - model, - numerical_time_series, - dynamics_type_str=dynamics_type, - ode_solver=ode_solver, + # dynamics = set_dynamics( + # model, + # numerical_time_series, + # dynamics_type_str=dynamics_type, + # ode_solver=ode_solver, + # ) + dynamics_options = DynamicsOptionsList() + dynamics_options.add( + DynamicsOptions( + expand_dynamics=True, + expand_continuity=False, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + ode_solver=ode_solver, + numerical_data_timeseries=numerical_time_series, + ) ) # Set initial guess for state variables @@ -484,9 +497,9 @@ def prepare_ocp( return OptimalControlProgram( [model], - dynamics, n_shooting, final_time, + dynamics_options, x_bounds=x_bounds, u_bounds=u_bounds, x_init=x_init, @@ -561,6 +574,7 @@ def main( activate_passive_force_relationship=True, activate_residual_torque=False, external_force_set=None, # External forces will be added later + with_contact=True ) # Adjust n_shooting based on the stimulation time n_shooting = model.muscles_dynamics_model[0].get_n_shooting(final_time) @@ -576,7 +590,7 @@ def main( dynamics_type=dynamics_type, use_sx=False, ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau"), - # ode_solver=OdeSolver.RK4(n_integration_steps=5) + # ode_solver=OdeSolver.RK4(n_integration_steps=5), torque=-0.3, initial_guess_model_path=initial_guess_model_path, ) diff --git a/external/bioptim b/external/bioptim index ec85e049..108a490e 160000 --- a/external/bioptim +++ b/external/bioptim @@ -1 +1 @@ -Subproject commit ec85e0497d49cf21f1d79168a70014b039b45802 +Subproject commit 108a490eeae6137f47808f24d1689af1dc8fcfb0 diff --git a/tests/shard1/test_models_dynamics_without_bioptim.py b/tests/shard1/test_models_dynamics_without_bioptim.py index f7bf376b..e85287e4 100644 --- a/tests/shard1/test_models_dynamics_without_bioptim.py +++ b/tests/shard1/test_models_dynamics_without_bioptim.py @@ -9,7 +9,7 @@ def test_ding2003_dynamics(): model = ModelMaker.create_model("ding2003_with_fatigue") assert model.nb_state == 5 - assert model.name_dof == ["Cn", "F", "A", "Tau1", "Km"] + assert model.name_dofs == ["Cn", "F", "A", "Tau1", "Km"] np.testing.assert_almost_equal( model.standard_rest_values(), np.array([[0], [0], [model.a_rest], [model.tau1_rest], [model.km_rest]]), @@ -73,7 +73,7 @@ def test_ding2003_dynamics(): def test_ding2007_dynamics(): model = ModelMaker.create_model("ding2007_with_fatigue") assert model.nb_state == 5 - assert model.name_dof == [ + assert model.name_dofs == [ "Cn", "F", "A", @@ -156,7 +156,7 @@ def test_ding2007_dynamics(): def test_hmed2018_dynamics(): model = ModelMaker.create_model("hmed2018_with_fatigue") assert model.nb_state == 5 - assert model.name_dof == [ + assert model.name_dofs == [ "Cn", "F", "A", @@ -245,7 +245,7 @@ def test_hmed2018_dynamics(): def test_veltink1992_dynamics(): model = ModelMaker.create_model("veltink_and_riener1998") assert model.nb_state == 2 - assert model.name_dof == [ + assert model.name_dofs == [ "a", "mu", ] @@ -296,7 +296,7 @@ def test_veltink1992_dynamics(): def test_marion2009_dynamics(): model = ModelMaker.create_model("marion2009_with_fatigue") assert model.nb_state == 5 - assert model.name_dof == [ + assert model.name_dofs == [ "Cn", "F", "A", @@ -362,7 +362,7 @@ def test_marion2009_dynamics(): def test_marion2009_modified_dynamics(): model = ModelMaker.create_model("marion2009_modified_with_fatigue") assert model.nb_state == 5 - assert model.name_dof == [ + assert model.name_dofs == [ "Cn", "F", "A", @@ -432,7 +432,7 @@ def test_marion2009_modified_dynamics(): def test_marion2013_dynamics(): model = ModelMaker.create_model("marion2013_with_fatigue") assert model.nb_state == 7 - assert model.name_dof == [ + assert model.name_dofs == [ "Cn", "F", "theta", @@ -524,7 +524,7 @@ def test_marion2013_dynamics(): def test_marion2013_modified_dynamics(): model = ModelMaker.create_model("marion2013_modified_with_fatigue") assert model.nb_state == 7 - assert model.name_dof == [ + assert model.name_dofs == [ "Cn", "F", "theta", diff --git a/tests/shard2/test_run_examples.py b/tests/shard2/test_run_examples.py index 989fff1e..2fb22924 100644 --- a/tests/shard2/test_run_examples.py +++ b/tests/shard2/test_run_examples.py @@ -74,15 +74,15 @@ def test_multibody_examples(module_name): cycling_biorbd_model_path = biomodel_folder + "/Wu/Modified_Wu_Shoulder_Model_Cycling.bioMod" cycling_initial_guess_biorbd_model_path = biomodel_folder + "/Wu/Modified_Wu_Shoulder_Model_Cycling_for_IK.bioMod" -# TODO: Update it -# @pytest.mark.parametrize("module_name", MULTIBODY_CYCLING_EXAMPLE_MODULES) -# def test_cycling_multibody_examples(module_name): -# ocp_module = importlib.import_module(module_name) -# ocp_module.main( -# plot=False, -# model_path=cycling_biorbd_model_path, -# initial_guess_model_path=cycling_initial_guess_biorbd_model_path, -# ) + +@pytest.mark.parametrize("module_name", MULTIBODY_CYCLING_EXAMPLE_MODULES) +def test_cycling_multibody_examples(module_name): + ocp_module = importlib.import_module(module_name) + ocp_module.main( + plot=False, + model_path=cycling_biorbd_model_path, + initial_guess_model_path=cycling_initial_guess_biorbd_model_path, + ) MULTIBODY_FLEXION_EXAMPLE_MODULES = [ From e9a211ed2eb1af30683a324159a6fcd6e899fc05 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 21 Nov 2025 13:41:18 -0500 Subject: [PATCH 3/5] black --- cocofest/integration/ivp_fes.py | 5 +- cocofest/models/ding2003/ding2003.py | 5 +- .../models/ding2003/ding2003_with_fatigue.py | 2 +- .../models/ding2007/ding2007_with_fatigue.py | 2 +- cocofest/models/dynamical_model.py | 46 +- cocofest/models/fes_model.py | 2 - cocofest/models/hmed2018/hmed2018.py | 4 +- .../models/hmed2018/hmed2018_with_fatigue.py | 34 +- .../models/marion2009/marion2009_modified.py | 2 +- .../marion2009_modified_with_fatigue.py | 4 +- .../marion2009/marion2009_with_fatigue.py | 2 +- cocofest/models/marion2013/marion2013.py | 2 +- .../models/marion2013/marion2013_modified.py | 2 +- .../marion2013_modified_with_fatigue.py | 2 +- .../marion2013/marion2013_with_fatigue.py | 18 +- cocofest/models/veltink1992/veltink1992.py | 3 +- .../veltink1992/veltink1992_and_riener1998.py | 2 +- .../fes_multibody/cycling/cost_functions.py | 773 ++++++++++++++++++ .../cycling/cycling_bayesian_mhe.py | 5 +- .../cycling/cycling_pulse_width_mhe.py | 5 +- .../cycling_with_different_driven_methods.py | 4 +- .../elbow_flexion/elbow_flexion_task.py | 5 +- .../frequency_optimization_multibody.py | 5 +- .../fes_multibody/reaching/reaching_task.py | 6 +- .../identification/muscle_model_id.py | 5 +- .../optimization/force_tracking.py | 9 +- .../optimization/frequency_optimization.py | 20 +- .../pulse_intensity_optimization.py | 5 +- .../optimization/pulse_width_optimization.py | 5 +- .../pulse_width_optimization_nmpc.py | 5 +- .../force_model/ding2003_model_id.py | 5 +- .../force_model/ding2007_model_id.py | 5 +- .../force_model/hmed2018_model_id.py | 5 +- .../other_fes_models/marion2009_example.py | 5 +- .../other_fes_models/marion2013_example.py | 5 +- .../other_fes_models/veltink1992_example.py | 11 +- .../muscle_relationships_comparison.py | 51 +- 37 files changed, 937 insertions(+), 139 deletions(-) create mode 100644 examples/fes_multibody/cycling/cost_functions.py diff --git a/cocofest/integration/ivp_fes.py b/cocofest/integration/ivp_fes.py index c858202b..dd3dc838 100644 --- a/cocofest/integration/ivp_fes.py +++ b/cocofest/integration/ivp_fes.py @@ -90,8 +90,9 @@ def __init__( self.n_shooting, self.final_time ) self.ode_solver = self.ivp_parameters["ode_solver"] - self.dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=self.ode_solver) + self.dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=self.ode_solver + ) ( self.x_init, diff --git a/cocofest/models/ding2003/ding2003.py b/cocofest/models/ding2003/ding2003.py index 3edde612..a2b753ed 100644 --- a/cocofest/models/ding2003/ding2003.py +++ b/cocofest/models/ding2003/ding2003.py @@ -40,7 +40,7 @@ def __init__( stim_time: list[float] = None, previous_stim: dict = None, sum_stim_truncation: int = 20, - **kwargs + **kwargs, ): super().__init__(name=model_name, **kwargs) self._model_name = model_name @@ -77,7 +77,6 @@ def __init__( self.force_velocity_relationship = 1 self.passive_force_relationship = 0 - # --- Configure variables --- # @property def state_configuration_functions(self) -> List[States | Callable]: @@ -135,7 +134,7 @@ def serialize(self) -> tuple[Callable, dict]: # ---- Needed for the example ---- # @property def name_dofs(self) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return ["Cn" + muscle_name, "F" + muscle_name] @property diff --git a/cocofest/models/ding2003/ding2003_with_fatigue.py b/cocofest/models/ding2003/ding2003_with_fatigue.py index 0ce2a51c..9cfc111f 100644 --- a/cocofest/models/ding2003/ding2003_with_fatigue.py +++ b/cocofest/models/ding2003/ding2003_with_fatigue.py @@ -94,7 +94,7 @@ def serialize(self) -> tuple[Callable, dict]: # ---- Needed for the example ---- # @property def name_dofs(self) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/ding2007/ding2007_with_fatigue.py b/cocofest/models/ding2007/ding2007_with_fatigue.py index 2d56f059..da40b8c6 100644 --- a/cocofest/models/ding2007/ding2007_with_fatigue.py +++ b/cocofest/models/ding2007/ding2007_with_fatigue.py @@ -53,7 +53,7 @@ def __init__( # ---- Absolutely needed methods ---- # @property def name_dofs(self) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 235915ef..aaef74f9 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -14,7 +14,7 @@ OptimalControlProgram, ParameterList, StateDynamics, - States + States, ) from ..models.fes_model import FesModel @@ -28,6 +28,7 @@ muscle_passive_force_coefficient, ) + class FesMskModel(BiorbdModel, StateDynamics): def __init__( self, @@ -67,10 +68,12 @@ def __init__( The parameters that will be used in the model """ self.with_contact = with_contact - super().__init__(bio_model=biorbd_path, - parameters=parameters, - external_force_set=external_force_set, - contact_types=[ContactType.RIGID_EXPLICIT] if self.with_contact else []) + super().__init__( + bio_model=biorbd_path, + parameters=parameters, + external_force_set=external_force_set, + contact_types=[ContactType.RIGID_EXPLICIT] if self.with_contact else [], + ) self._name = name self.biorbd_path = biorbd_path @@ -99,7 +102,6 @@ def __init__( self.external_forces_set = external_force_set self.muscles_model = muscles_model - @property def state_configuration_functions(self) -> List[States | Callable]: return [StateConfigure().configure_all_muscle_msk_states, States.Q, States.QDOT] @@ -226,7 +228,9 @@ def dynamics( # 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 - external_forces = nlp.get_external_forces("external_forces", states, controls, algebraic_states, numerical_data_timeseries) + external_forces = nlp.get_external_forces( + "external_forces", states, controls, algebraic_states, numerical_data_timeseries + ) ddq = nlp.model.forward_dynamics(with_contact=self.with_contact)( q, qdot, total_torque, external_forces, parameters @@ -258,7 +262,7 @@ def muscles_joint_torque( dxdt_muscle_list = vertcat() muscle_forces = vertcat() muscle_idx_list = [] - state_name_list=nlp.states.keys() + state_name_list = nlp.states.keys() Q = nlp.model.q Qdot = nlp.model.qdot @@ -345,12 +349,19 @@ def muscles_joint_torque( ) muscle_model.fes_model = muscle_model - muscle_model.force_length_relationship=muscle_force_length_coeff - muscle_model.force_velocity_relationship=muscle_force_velocity_coeff - muscle_model.passive_force_relationship=muscle_passive_force_coeff - - muscle_dxdt = muscle_model.dynamics(time, muscle_states, muscle_controls, muscle_parameters, - algebraic_states, fes_numerical_data_timeseries, nlp).dxdt + muscle_model.force_length_relationship = muscle_force_length_coeff + muscle_model.force_velocity_relationship = muscle_force_velocity_coeff + muscle_model.passive_force_relationship = muscle_passive_force_coeff + + muscle_dxdt = muscle_model.dynamics( + time, + muscle_states, + muscle_controls, + muscle_parameters, + algebraic_states, + fes_numerical_data_timeseries, + nlp, + ).dxdt dxdt_muscle_list = vertcat(dxdt_muscle_list, muscle_dxdt) muscle_idx_list.append(muscle_idx) @@ -437,16 +448,15 @@ def forces_from_fes_driven( return nlp.model.rigid_contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) - @staticmethod def declare_model_control( ocp: OptimalControlProgram, nlp: NonLinearProgram, numerical_data_timeseries: dict[str, np.ndarray] = None, contact_type: list = (), - as_states: bool=False, - as_controls: bool=True, - as_algebraic_states: bool=False, + as_states: bool = False, + as_controls: bool = True, + as_algebraic_states: bool = False, ): """ Tell the program which variables are states and controls. diff --git a/cocofest/models/fes_model.py b/cocofest/models/fes_model.py index 268263eb..b0218078 100644 --- a/cocofest/models/fes_model.py +++ b/cocofest/models/fes_model.py @@ -113,8 +113,6 @@ def system_dynamics( controls: MX, numerical_timeseries: MX, ): - - """ Returns diff --git a/cocofest/models/hmed2018/hmed2018.py b/cocofest/models/hmed2018/hmed2018.py index 6116fe1b..6dde979c 100644 --- a/cocofest/models/hmed2018/hmed2018.py +++ b/cocofest/models/hmed2018/hmed2018.py @@ -8,6 +8,7 @@ from cocofest.models.ding2003.ding2003 import DingModelFrequency from cocofest.models.state_configure import StateConfigure + class DingModelPulseIntensityFrequency(DingModelFrequency): """ This is a custom models that inherits from bioptim. CustomModel. @@ -57,7 +58,6 @@ def __init__( self.cr = CR_DEFAULT self.impulse_intensity = None - @property def control_configuration_functions(self) -> List[States | Callable]: return [StateConfigure().configure_pulse_intensity] @@ -77,7 +77,7 @@ def identifiable_parameters(self): @property def pulse_intensity_name(self): - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return "pulse_intensity" + muscle_name def set_ar(self, model, ar: MX | float): diff --git a/cocofest/models/hmed2018/hmed2018_with_fatigue.py b/cocofest/models/hmed2018/hmed2018_with_fatigue.py index d0b777b7..39810918 100644 --- a/cocofest/models/hmed2018/hmed2018_with_fatigue.py +++ b/cocofest/models/hmed2018/hmed2018_with_fatigue.py @@ -54,7 +54,7 @@ def __init__( # ---- Absolutely needed methods ---- # @property def name_dofs(self) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, @@ -122,22 +122,22 @@ def system_dynamics( numerical_timeseries: MX, ) -> MX: """ - The system dynamics is the function that describes the models. - - Parameters - ---------- - time: MX - The system's current node time - states: MX - The state of the system CN, F, A, Tau1, Km - controls: MX - The controls of the system, pulse_intensity - numerical_timeseries: MX - The numerical timeseries of the system - - Returns - ------- - The value of the derivative of each state dx/dt at the current time t + The system dynamics is the function that describes the models. + + Parameters + ---------- + time: MX + The system's current node time + states: MX + The state of the system CN, F, A, Tau1, Km + controls: MX + The controls of the system, pulse_intensity + numerical_timeseries: MX + The numerical timeseries of the system + + Returns + ------- + The value of the derivative of each state dx/dt at the current time t """ t = time cn = states[0] diff --git a/cocofest/models/marion2009/marion2009_modified.py b/cocofest/models/marion2009/marion2009_modified.py index f22582a1..acef62d5 100644 --- a/cocofest/models/marion2009/marion2009_modified.py +++ b/cocofest/models/marion2009/marion2009_modified.py @@ -131,7 +131,7 @@ def system_dynamics( cn = states[0] f = states[1] pulse_width = controls[0] - theta=controls[1] if controls.shape[0] > 1 else 90 + theta = controls[1] if controls.shape[0] > 1 else 90 t_stim_prev = numerical_timeseries cn_dot = self.calculate_cn_dot(cn, t, t_stim_prev) diff --git a/cocofest/models/marion2009/marion2009_modified_with_fatigue.py b/cocofest/models/marion2009/marion2009_modified_with_fatigue.py index 6d188776..427b4b24 100644 --- a/cocofest/models/marion2009/marion2009_modified_with_fatigue.py +++ b/cocofest/models/marion2009/marion2009_modified_with_fatigue.py @@ -55,7 +55,7 @@ def __init__( @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, @@ -131,7 +131,7 @@ def system_dynamics( f = states[1] a = states[2] tau1 = states[3] - km=states[4] + km = states[4] pulse_width = controls[0] theta = controls[1] if controls.shape[0] > 1 else 90 t_stim_prev = numerical_timeseries diff --git a/cocofest/models/marion2009/marion2009_with_fatigue.py b/cocofest/models/marion2009/marion2009_with_fatigue.py index e67c35e1..c4a919df 100644 --- a/cocofest/models/marion2009/marion2009_with_fatigue.py +++ b/cocofest/models/marion2009/marion2009_with_fatigue.py @@ -52,7 +52,7 @@ def __init__( @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013.py b/cocofest/models/marion2013/marion2013.py index 8c56d341..45295f39 100644 --- a/cocofest/models/marion2013/marion2013.py +++ b/cocofest/models/marion2013/marion2013.py @@ -61,7 +61,7 @@ def __init__( @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013_modified.py b/cocofest/models/marion2013/marion2013_modified.py index 7bb9615f..ede8afb4 100644 --- a/cocofest/models/marion2013/marion2013_modified.py +++ b/cocofest/models/marion2013/marion2013_modified.py @@ -69,7 +69,7 @@ def __init__( @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013_modified_with_fatigue.py b/cocofest/models/marion2013/marion2013_modified_with_fatigue.py index 46e8c71a..4d639d70 100644 --- a/cocofest/models/marion2013/marion2013_modified_with_fatigue.py +++ b/cocofest/models/marion2013/marion2013_modified_with_fatigue.py @@ -59,7 +59,7 @@ def __init__( @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, diff --git a/cocofest/models/marion2013/marion2013_with_fatigue.py b/cocofest/models/marion2013/marion2013_with_fatigue.py index 5e992204..beeeb0af 100644 --- a/cocofest/models/marion2013/marion2013_with_fatigue.py +++ b/cocofest/models/marion2013/marion2013_with_fatigue.py @@ -56,7 +56,7 @@ def __init__( @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "Cn" + muscle_name, "F" + muscle_name, @@ -183,14 +183,14 @@ def system_dynamics( The value of the derivative of each state dx/dt at the current time t """ t = time - cn=states[0] - f=states[1] - theta=states[2] - dtheta_dt=states[3] - a=states[4] - tau1=states[5] - km=states[6] - Fload=controls[0] if controls.shape[0] > 0 else 0.0 + cn = states[0] + f = states[1] + theta = states[2] + dtheta_dt = states[3] + a = states[4] + tau1 = states[5] + km = states[6] + Fload = controls[0] if controls.shape[0] > 0 else 0.0 t_stim_prev = numerical_timeseries # Get CN dynamics from Ding model diff --git a/cocofest/models/veltink1992/veltink1992.py b/cocofest/models/veltink1992/veltink1992.py index dc10f2f2..80243479 100644 --- a/cocofest/models/veltink1992/veltink1992.py +++ b/cocofest/models/veltink1992/veltink1992.py @@ -42,7 +42,6 @@ def __init__( self.contact_types = () - @property def name(self): return self._name @@ -66,7 +65,7 @@ def extra_configuration_functions(self) -> List[States | Callable]: @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return ["a" + muscle_name] # Only muscle activation state @property diff --git a/cocofest/models/veltink1992/veltink1992_and_riener1998.py b/cocofest/models/veltink1992/veltink1992_and_riener1998.py index 2bbcef4c..8b5aaf4d 100644 --- a/cocofest/models/veltink1992/veltink1992_and_riener1998.py +++ b/cocofest/models/veltink1992/veltink1992_and_riener1998.py @@ -50,7 +50,7 @@ def __init__( @property def name_dofs(self, with_muscle_name: bool = False) -> list[str]: - muscle_name = ("_" + self.muscle_name if self.muscle_name is not None else "") + muscle_name = "_" + self.muscle_name if self.muscle_name is not None else "" return [ "a" + muscle_name, # Muscle activation "mu" + muscle_name, # Fatigue state diff --git a/examples/fes_multibody/cycling/cost_functions.py b/examples/fes_multibody/cycling/cost_functions.py new file mode 100644 index 00000000..885a2522 --- /dev/null +++ b/examples/fes_multibody/cycling/cost_functions.py @@ -0,0 +1,773 @@ +from casadi import MX, vertcat, sum1, mmax, fabs, sign +from bioptim import PenaltyController +from cocofest.models.ding2007.ding2007 import DingModelPulseWidthFrequency + + +class CustomCostFunctions: + def __init__(self): + self.dict_functions = { + "minimize_average_activation": { + "function": self.minimize_average_activation, + "index": 1, + "latex": r"\phi_1 = \frac{1}{n_m}\sum_{j=1}^{n_m} a^{j}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", + "description": "Minimize the average muscle activation", + "power": "1", + "state": "pw", + }, + "minimize_root_mean_square_activation": { + "function": self.minimize_root_mean_square_activation, + "index": 2, + "latex": r"\phi_2 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (a^{j})^{2}\right)^{\tfrac{1}{2}}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", + "description": "Minimize the root mean square of muscle activation", + "power": "2", + "state": "pw", + }, + "minimize_cubic_average_activation": { + "function": self.minimize_cubic_average_activation, + "index": 3, + "latex": r"\phi_3 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (a^{j})^{3}\right)^{\tfrac{1}{3}}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", + "description": "Minimize the cubic average of muscle activation", + "power": "3", + "state": "pw", + }, + "minimize_peak_activation": { + "function": self.minimize_peak_activation, + "index": 4, + "latex": r"\phi_4 = \max_{j=1,\ldots,n_m} \; a^{j}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", + "description": "Minimize the peak of muscle activation", + "power": "max", + "state": "pw", + }, + "minimize_average_force": { + "function": self.minimize_average_force, + "index": 5, + "latex": r"\phi_5 = \frac{1}{n_m}\sum_{j=1}^{n_m} f^{j}", + "description": "Minimize the average muscle force", + "power": "1", + "state": "f", + }, + "minimize_root_mean_square_force": { + "function": self.minimize_root_mean_square_force, + "index": 6, + "latex": r"\phi_6 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j})^{2}\right)^{\tfrac{1}{2}}", + "description": "Minimize the root mean square of muscle force", + "power": "2", + "state": "f", + }, + "minimize_cubic_average_force": { + "function": self.minimize_cubic_average_force, + "index": 7, + "latex": r"\phi_7 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j})^{3}\right)^{\tfrac{1}{3}}", + "description": "Minimize the cubic average of muscle force", + "power": "3", + "state": "f", + }, + "minimize_peak_force": { + "function": self.minimize_peak_force, + "index": 8, + "latex": r"\phi_8 = \max_{j=1,\ldots,n_m} \; f^{j}", + "description": "Minimize the peak muscle force", + "power": "max", + "state": "f", + }, + "minimize_average_muscle_stress": { + "function": self.minimize_average_muscle_stress, + "index": 9, + "latex": r"\phi_9 = \frac{1}{n_m}\sum_{j=1}^{n_m} \frac{f^{j}}{S^{j}}", + "description": "Minimize the average muscle stress", + "power": "1", + "state": "str", + }, + "minimize_root_mean_square_muscle_stress": { + "function": self.minimize_root_mean_square_muscle_stress, + "index": 10, + "latex": r"\phi_{10} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} \left(\frac{f^{j}}{S^{j}}\right)^{2}\right)^{\tfrac{1}{2}}", + "description": "Minimize the root mean square of muscle stress", + "power": "2", + "state": "str", + }, + "minimize_cubic_average_muscle_stress": { + "function": self.minimize_cubic_average_muscle_stress, + "index": 11, + "latex": r"\phi_{11} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} \left(\frac{f^{j}}{S^{j}}\right)^{3}\right)^{\tfrac{1}{3}}", + "description": "Minimize the cubic average of muscle stress", + "power": "3", + "state": "str", + }, + "minimize_peak_muscle_stress": { + "function": self.minimize_peak_muscle_stress, + "index": 12, + "latex": r"\phi_{12} = \max_{j=1,\ldots,n_m} \; \frac{f^{j}}{S^{j}}", + "description": "Minimize the peak muscle stress", + "power": "max", + "state": "str", + }, + "minimize_average_muscle_power": { + "function": self.minimize_average_muscle_power, + "index": 13, + "latex": r"\phi_{13} = \frac{1}{n_m}\sum_{j=1}^{n_m} f^{j} v^{j}", + "description": "Minimize the average muscle power", + "power": "1", + "state": "pow", + }, + "minimize_root_mean_square_muscle_power": { + "function": self.minimize_root_mean_square_muscle_power, + "index": 14, + "latex": r"\phi_{14} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j} v^{j})^{2}\right)^{\tfrac{1}{2}}", + "description": "Minimize the root mean square of muscle power", + "power": "2", + "state": "pow", + }, + "minimize_cubic_average_muscle_power": { + "function": self.minimize_cubic_average_muscle_power, + "index": 15, + "latex": r"\phi_{15} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j} v^{j})^{3}\right)^{\tfrac{1}{3}}", + "description": "Minimize the cubic average of muscle power", + "power": "3", + "state": "pow", + }, + "minimize_peak_muscle_power": { + "function": self.minimize_peak_muscle_power, + "index": 16, + "latex": r"\phi_{16} = \max_{j=1,\ldots,n_m} \; f^{j} v^{j}}", + "description": "Minimize the peak muscle power", + "power": "max", + "state": "pow", + }, + "minimize_average_fatigue": { + "function": self.minimize_average_fatigue, + "index": 17, + "latex": r"\phi_{17} = \frac{1}{n_m}\sum_{j=1}^{n_m} \mathcal{F}^{j}", + "description": "Minimize the average muscle fatigue", + "power": "1", + "state": "fat", + }, + "minimize_root_mean_square_fatigue": { + "function": self.minimize_root_mean_square_fatigue, + "index": 18, + "latex": r"\phi_{18} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (\mathcal{F}^{j})^{2}\right)^{\tfrac{1}{2}}", + "description": "Minimize the root mean square of muscle fatigue", + "power": "2", + "state": "fat", + }, + "minimize_cubic_average_fatigue": { + "function": self.minimize_cubic_average_fatigue, + "index": 19, + "latex": r"\phi_{19} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (\mathcal{F}^{j})^{3}\right)^{\tfrac{1}{3}}", + "description": "Minimize the cubic average of muscle fatigue", + "power": "3", + "state": "fat", + }, + "minimize_peak_fatigue": { + "function": self.minimize_peak_fatigue, + "index": 20, + "latex": r"\phi_{20} = \max_{j=1,\ldots,n_m} \; \mathcal{F}^{j}", + "description": "Minimize the peak muscle fatigue", + "power": "max", + "state": "fat", + }, + "minimize_peak": { + "function": self.minimize_peak, + "index": 99, + "latex": r"\phi_{99} = \max_{j=1,\ldots,n_m} \; \mathcal{Var}^{j}", + "description": "Minimize the peak of a variable", + }, + } + + # --- Electrical stimulation cost functions --- # + @staticmethod + def minimize_average_activation(controller: PenaltyController) -> MX: + """ + Minimize the average muscle activation. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The average of muscle activation + """ + muscle_name_list = controller.model.bio_model.muscle_names + if isinstance(controller.model.muscles_dynamics_model[0], DingModelPulseWidthFrequency): + 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]].min[0][0] + ) + / ( + controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] + - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] + ) + for x in range(len(muscle_name_list)) + ] + ) + else: + raise NotImplementedError( + "Minimizing average activation is only implemented for DingModelPulseWidthFrequency." + ) + + return sum1(stim_charge) / len(muscle_name_list) + + @staticmethod + def minimize_root_mean_square_activation(controller: PenaltyController) -> MX: + """ + Minimize the root-mean-square of muscle activation. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The root-mean-square of muscle activation + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + if isinstance(controller.model.muscles_dynamics_model[0], DingModelPulseWidthFrequency): + 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]].min[0][0] + ) + / ( + controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] + - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] + ) + ) + ** 2 + for x in range(len(muscle_name_list)) + ] + ) + else: + raise NotImplementedError( + "Minimizing average activation is only implemented for DingModelPulseWidthFrequency." + ) + + rms_activation = (sum1(stim_charge) / len(muscle_name_list) + eps) ** 0.5 + return rms_activation + + @staticmethod + def minimize_cubic_average_activation(controller: PenaltyController) -> MX: + """ + Minimize the cubic average of muscle activation. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The cubic average of muscle activation + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + if isinstance(controller.model.muscles_dynamics_model[0], DingModelPulseWidthFrequency): + 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]].min[0][0] + ) + / ( + controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] + - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] + ) + ) + ** 3 + for x in range(len(muscle_name_list)) + ] + ) + else: + raise NotImplementedError( + "Minimizing average activation is only implemented for DingModelPulseWidthFrequency." + ) + + x = sum1(stim_charge) / len(muscle_name_list) + cubic_avg_activation = sign(x) * (fabs(x) + eps) ** (1 / 3) + # cubic_avg_activation = (sum1(stim_charge) / len(muscle_name_list) + eps) ** (1/3) + return cubic_avg_activation + + @staticmethod + def minimize_peak_activation(controller: PenaltyController) -> MX: + """ + Minimize the peak muscle activation. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The peak of muscle activation + """ + muscle_name_list = controller.model.bio_model.muscle_names + stim_activation = vertcat( + *[ + ( + controller.controls["last_pulse_width_" + muscle_name_list[x]].cx + - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] + ) + / ( + controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] + - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] + ) + for x in range(len(muscle_name_list)) + ] + ) + max_activation = mmax(stim_activation) + return max_activation + + # --- Muscle force cost functions --- # + @staticmethod + def minimize_average_force(controller: PenaltyController) -> MX: + """ + Minimize the average muscle force production. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The average of produced force + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_force = vertcat( + *[controller.states["F_" + muscle_name_list[x]].cx for x in range(len(muscle_name_list))] + ) + return sum1(muscle_force) / len(muscle_name_list) + + @staticmethod + def minimize_root_mean_square_force(controller: PenaltyController) -> MX: + """ + Minimize the root-mean-square of muscle force production. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The root-mean-square of produced force + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_force = vertcat( + *[controller.states["F_" + muscle_name_list[x]].cx ** 2 for x in range(len(muscle_name_list))] + ) + rms_force = (sum1(muscle_force) / len(muscle_name_list) + eps) ** 0.5 + return rms_force + + @staticmethod + def minimize_cubic_average_force(controller: PenaltyController) -> MX: + """ + Minimize the cubic average of muscle force production. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The cubic average of produced force + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_force = vertcat( + *[controller.states["F_" + muscle_name_list[x]].cx ** 3 for x in range(len(muscle_name_list))] + ) + cubic_avg_force = (sum1(muscle_force) / len(muscle_name_list) + eps) ** (1 / 3) + return cubic_avg_force + + @staticmethod + def minimize_peak_force(controller: PenaltyController) -> MX: + """ + Minimize the peak muscle force production. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The peak of produced force + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_force = vertcat( + *[controller.states["F_" + muscle_name_list[x]].cx for x in range(len(muscle_name_list))] + ) + max_force = mmax(muscle_force) + return max_force + + # --- Muscle stress cost functions --- # + @staticmethod + def minimize_average_muscle_stress(controller: PenaltyController) -> MX: + """ + Minimize the average muscle stress. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The average of muscle stress + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_stress = vertcat( + *[ + controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa + for x in range(len(muscle_name_list)) + ] + ) + return sum1(muscle_stress) / len(muscle_name_list) + + @staticmethod + def minimize_root_mean_square_muscle_stress(controller: PenaltyController) -> MX: + """ + Minimize the root-mean-square of muscle stress. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The root-mean-square of muscle stress + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_stress = vertcat( + *[ + (controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa) + ** 2 + for x in range(len(muscle_name_list)) + ] + ) + rms_stress = (sum1(muscle_stress) / len(muscle_name_list) + eps) ** 0.5 + return rms_stress + + @staticmethod + def minimize_cubic_average_muscle_stress(controller: PenaltyController) -> MX: + """ + Minimize the cubic average of muscle stress. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The cubic average of muscle stress + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_stress = vertcat( + *[ + (controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa) + ** 3 + for x in range(len(muscle_name_list)) + ] + ) + cubic_avg_stress = (sum1(muscle_stress) / len(muscle_name_list) + eps) ** (1 / 3) + return cubic_avg_stress + + @staticmethod + def minimize_peak_muscle_stress(controller: PenaltyController) -> MX: + """ + Minimize the peak muscle stress. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The peak of muscle stress + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_stress = vertcat( + *[ + controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa + for x in range(len(muscle_name_list)) + ] + ) + max_stress = mmax(muscle_stress) + return max_stress + + # --- Muscle power cost functions --- # + @staticmethod + def minimize_average_muscle_power(controller: PenaltyController) -> MX: + """ + Minimize the average of muscle power. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The average of muscle power + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_velocity = controller.model.muscle_velocity()( + controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx + ) + muscle_power = vertcat( + *[ + (controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) + for x in range(len(muscle_name_list)) + ] + ) + return sum1(muscle_power) / len(muscle_name_list) + + @staticmethod + def minimize_root_mean_square_muscle_power(controller: PenaltyController) -> MX: + """ + Minimize the root-mean-square of muscle power. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The root-mean-square of muscle power + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_velocity = controller.model.muscle_velocity()( + controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx + ) + muscle_power = vertcat( + *[ + (controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) ** 2 + for x in range(len(muscle_name_list)) + ] + ) + rms_power = (sum1(muscle_power) / len(muscle_name_list) + eps) ** 0.5 + return rms_power + + @staticmethod + def minimize_cubic_average_muscle_power(controller: PenaltyController) -> MX: + """ + Minimize the cubic average of muscle power. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The cubic average of muscle power + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_velocity = controller.model.muscle_velocity()( + controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx + ) + muscle_power = vertcat( + *[ + ((controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) ** 2 + 1e-6) ** 1.5 + for x in range(len(muscle_name_list)) + ] + ) + cubic_avg_power = ((sum1(muscle_power)) / len(muscle_name_list) + eps) ** (1 / 3) + return cubic_avg_power + + @staticmethod + def minimize_peak_muscle_power(controller: PenaltyController) -> MX: + """ + Minimize the peak muscle power. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The peak of muscle power + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_velocity = controller.model.muscle_velocity()( + controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx + ) + muscle_power = vertcat( + *[ + (controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) + for x in range(len(muscle_name_list)) + ] + ) + max_power = mmax(muscle_power) + return max_power + + # --- Muscle fatigue cost functions --- # + @staticmethod + def minimize_average_fatigue(controller: PenaltyController) -> MX: + """ + Minimize the average muscle fatigue. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The average of muscle fatigue + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_fatigue = vertcat( + *[ + controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx + for x in range(len(muscle_name_list)) + ] + ) + return sum1(muscle_fatigue) / len(muscle_name_list) + + @staticmethod + def minimize_root_mean_square_fatigue(controller: PenaltyController) -> MX: + """ + Minimize the root-mean-square of muscle fatigue. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The root-mean-square of muscle fatigue + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_fatigue = vertcat( + *[ + (controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx) + ** 2 + for x in range(len(muscle_name_list)) + ] + ) + rms_fatigue = (sum1(muscle_fatigue) / len(muscle_name_list) + eps) ** 0.5 + return rms_fatigue + + @staticmethod + def minimize_cubic_average_fatigue(controller: PenaltyController) -> MX: + """ + Minimize the cubic average of muscle fatigue. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The cubic average of muscle fatigue + """ + eps = 1e-8 + muscle_name_list = controller.model.bio_model.muscle_names + muscle_fatigue = vertcat( + *[ + (controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx) + ** 3 + for x in range(len(muscle_name_list)) + ] + ) + cubic_avg_fatigue = (sum1(muscle_fatigue) / len(muscle_name_list) + eps) ** (1 / 3) + return cubic_avg_fatigue + + @staticmethod + def minimize_peak_fatigue(controller: PenaltyController) -> MX: + """ + Minimize the peak muscle fatigue. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The peak of muscle fatigue + """ + muscle_name_list = controller.model.bio_model.muscle_names + muscle_fatigue = vertcat( + *[ + controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx + for x in range(len(muscle_name_list)) + ] + ) + max_fatigue = mmax(muscle_fatigue) + return max_fatigue + + # --- Peak cost function and constraint used in OCP --- # + @staticmethod + def minimize_peak(controller: PenaltyController) -> MX: + """ + Minimize the peak of a variable. + + Parameters + ---------- + controller: PenaltyController + The penalty node elements + + Returns + ------- + The peak of a variable + """ + return controller.parameters["minmax_param"].cx + + @staticmethod + def constraints_minmax(controller: PenaltyController, obj_fun_key: str, param_index: int) -> MX: + muscle_name_list = controller.model.bio_model.muscle_names + + if obj_fun_key == ["minimize_peak_force"]: + value = controller.states["F_" + muscle_name_list[param_index]].cx + + elif obj_fun_key == ["minimize_peak_activation"]: + value = ( + controller.controls["last_pulse_width_" + muscle_name_list[param_index]].cx + - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[param_index]].min[0][0] + ) / ( + controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[param_index]].max[0][0] + - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[param_index]].min[0][0] + ) + + elif obj_fun_key == ["minimize_peak_muscle_stress"]: + value = ( + controller.states["F_" + muscle_name_list[param_index]].cx + / controller.model.muscles_dynamics_model[param_index].pcsa + ) + + elif obj_fun_key == ["minimize_peak_fatigue"]: + value = ( + controller.model.muscles_dynamics_model[param_index].a_scale + - controller.states["A_" + muscle_name_list[param_index]].cx + ) + + else: + raise NotImplementedError(f"The cost function {obj_fun_key}, is not implementend in minmax") + + return controller.parameters["minmax_param"].cx[controller.node_index] - value diff --git a/examples/fes_multibody/cycling/cycling_bayesian_mhe.py b/examples/fes_multibody/cycling/cycling_bayesian_mhe.py index 9ea09f99..bfb14520 100644 --- a/examples/fes_multibody/cycling/cycling_bayesian_mhe.py +++ b/examples/fes_multibody/cycling/cycling_bayesian_mhe.py @@ -117,8 +117,9 @@ def prepare_nmpc_bo( numerical_time_series.update(time_series2) # --- Dynamics & states --- # - dynamics_options = base.set_dynamics_options(numerical_time_series=numerical_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = base.set_dynamics_options( + numerical_time_series=numerical_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) x_init = base.set_q_qdot_init( n_shooting=window_n_shooting, diff --git a/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py b/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py index 8653993e..d5fbb1b2 100644 --- a/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py +++ b/examples/fes_multibody/cycling/cycling_pulse_width_mhe.py @@ -404,8 +404,9 @@ def set_external_forces(n_shooting, external_force_dict, force_name): def set_dynamics_options(numerical_time_series, ode_solver): - dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_time_series, - ode_solver=ode_solver) + dynamics_options = OcpFesMsk.declare_dynamics_options( + numerical_time_series=numerical_time_series, ode_solver=ode_solver + ) return dynamics_options diff --git a/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py b/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py index a3ed11e2..b6c00403 100644 --- a/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py +++ b/examples/fes_multibody/cycling/cycling_with_different_driven_methods.py @@ -95,7 +95,7 @@ def update_model( activate_residual_torque=model.activate_residual_torque, parameters=parameters, external_force_set=external_force_set, - with_contact=model.with_contact + with_contact=model.with_contact, ) else: model = BiorbdModel(model.path, external_force_set=external_force_set) @@ -574,7 +574,7 @@ def main( activate_passive_force_relationship=True, activate_residual_torque=False, external_force_set=None, # External forces will be added later - with_contact=True + with_contact=True, ) # Adjust n_shooting based on the stimulation time n_shooting = model.muscles_dynamics_model[0].get_n_shooting(final_time) diff --git a/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py b/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py index f327cb7b..0ba091e7 100644 --- a/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py +++ b/examples/fes_multibody/elbow_flexion/elbow_flexion_task.py @@ -42,8 +42,9 @@ def prepare_ocp( numerical_data_time_series, stim_idx_at_node_list = muscle_model.get_numerical_data_time_series( n_shooting, final_time ) - dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFesMsk.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) u_bounds, u_init = OcpFesMsk.set_u_bounds(model, msk_info["with_residual_torque"], max_bound=max_bound) diff --git a/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py b/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py index 3a8eebda..d5492244 100644 --- a/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py +++ b/examples/fes_multibody/elbow_flexion/frequency_optimization_multibody.py @@ -21,8 +21,9 @@ def prepare_ocp(model, final_time: float, resistive_torque, msk_info): n_shooting, final_time ) numerical_time_series.update(numerical_data_time_series) - dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFesMsk.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) diff --git a/examples/fes_multibody/reaching/reaching_task.py b/examples/fes_multibody/reaching/reaching_task.py index e1040f75..1ffe5a16 100644 --- a/examples/fes_multibody/reaching/reaching_task.py +++ b/examples/fes_multibody/reaching/reaching_task.py @@ -67,8 +67,10 @@ def prepare_ocp( numerical_data_time_series, stim_idx_at_node_list = muscle_model.get_numerical_data_time_series( n_shooting, final_time ) - dynamics_options = OcpFesMsk.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau")) + dynamics_options = OcpFesMsk.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, + ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau"), + ) # --- Initialize default FES bounds and initial guess --- # x_bounds, x_init_fes = OcpFesMsk.set_x_bounds_fes(model) diff --git a/examples/getting_started/identification/muscle_model_id.py b/examples/getting_started/identification/muscle_model_id.py index eab29c7c..889ccace 100644 --- a/examples/getting_started/identification/muscle_model_id.py +++ b/examples/getting_started/identification/muscle_model_id.py @@ -72,8 +72,9 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, diff --git a/examples/getting_started/optimization/force_tracking.py b/examples/getting_started/optimization/force_tracking.py index ae20957f..2c81c9ea 100644 --- a/examples/getting_started/optimization/force_tracking.py +++ b/examples/getting_started/optimization/force_tracking.py @@ -35,10 +35,11 @@ def prepare_ocp(model: FesModel, final_time: float, pw_max: float, force_trackin time_series, stim_idx_at_node_list = model.get_numerical_data_time_series( n_shooting, final_time ) # Retrieve time and indexes at which occurs the stimulation for the FES dynamic - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10) - # ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau"), # Possibility to use a different solver) - ) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=time_series, + ode_solver=OdeSolver.RK4(n_integration_steps=10), + # ode_solver=OdeSolver.COLLOCATION(polynomial_degree=3, method="radau"), # Possibility to use a different solver) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) x_init = OcpFes.set_x_init(model) diff --git a/examples/getting_started/optimization/frequency_optimization.py b/examples/getting_started/optimization/frequency_optimization.py index d38d6ee9..233c71cd 100644 --- a/examples/getting_started/optimization/frequency_optimization.py +++ b/examples/getting_started/optimization/frequency_optimization.py @@ -4,13 +4,14 @@ No optimization will be done on the stimulation, the frequency is fixed to 1Hz. """ -from bioptim import (ObjectiveList, - ObjectiveFcn, - OptimalControlProgram, - ControlType, - OdeSolver, - Node, - ) +from bioptim import ( + ObjectiveList, + ObjectiveFcn, + OptimalControlProgram, + ControlType, + OdeSolver, + Node, +) from cocofest import OcpFes, ModelMaker @@ -19,8 +20,9 @@ def prepare_ocp(model, final_time): n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) # --- Set initial guesses and bounds for states --- # x_bounds = OcpFes.set_x_bounds(model) diff --git a/examples/getting_started/optimization/pulse_intensity_optimization.py b/examples/getting_started/optimization/pulse_intensity_optimization.py index 126a0929..344e1c68 100644 --- a/examples/getting_started/optimization/pulse_intensity_optimization.py +++ b/examples/getting_started/optimization/pulse_intensity_optimization.py @@ -17,8 +17,9 @@ def prepare_ocp(model, final_time, pi_max): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) diff --git a/examples/getting_started/optimization/pulse_width_optimization.py b/examples/getting_started/optimization/pulse_width_optimization.py index 64ffc77e..1b0ceed1 100644 --- a/examples/getting_started/optimization/pulse_width_optimization.py +++ b/examples/getting_started/optimization/pulse_width_optimization.py @@ -13,8 +13,9 @@ def prepare_ocp(model, final_time, pw_max): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) diff --git a/examples/getting_started/optimization/pulse_width_optimization_nmpc.py b/examples/getting_started/optimization/pulse_width_optimization_nmpc.py index c8a4f94c..a92f343d 100644 --- a/examples/getting_started/optimization/pulse_width_optimization_nmpc.py +++ b/examples/getting_started/optimization/pulse_width_optimization_nmpc.py @@ -51,8 +51,9 @@ def prepare_nmpc( ) # dynamics = OcpFes.declare_dynamics(model, numerical_data_time_series, ode_solver) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=ode_solver) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=ode_solver + ) x_bounds = OcpFes.set_x_bounds(model) x_init_fes = OcpFes.set_x_init(model) diff --git a/examples/identification/force_model/ding2003_model_id.py b/examples/identification/force_model/ding2003_model_id.py index 676ea805..bdee3084 100644 --- a/examples/identification/force_model/ding2003_model_id.py +++ b/examples/identification/force_model/ding2003_model_id.py @@ -67,8 +67,9 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, diff --git a/examples/identification/force_model/ding2007_model_id.py b/examples/identification/force_model/ding2007_model_id.py index b681c1b9..f2c6acda 100644 --- a/examples/identification/force_model/ding2007_model_id.py +++ b/examples/identification/force_model/ding2007_model_id.py @@ -56,8 +56,9 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, diff --git a/examples/identification/force_model/hmed2018_model_id.py b/examples/identification/force_model/hmed2018_model_id.py index f0ca0062..0fd90ca8 100644 --- a/examples/identification/force_model/hmed2018_model_id.py +++ b/examples/identification/force_model/hmed2018_model_id.py @@ -50,8 +50,9 @@ def prepare_ocp( ) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) x_bounds, x_init = OcpFesId.set_x_bounds( model=model, diff --git a/examples/other_fes_models/marion2009_example.py b/examples/other_fes_models/marion2009_example.py index 8f383515..a0fd10d5 100644 --- a/examples/other_fes_models/marion2009_example.py +++ b/examples/other_fes_models/marion2009_example.py @@ -24,8 +24,9 @@ def prepare_ocp(model, final_time, pw_max=0.0006): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) diff --git a/examples/other_fes_models/marion2013_example.py b/examples/other_fes_models/marion2013_example.py index 99d8c667..59917adf 100644 --- a/examples/other_fes_models/marion2013_example.py +++ b/examples/other_fes_models/marion2013_example.py @@ -27,8 +27,9 @@ def prepare_ocp(model, final_time, pw_max=0.0006): # --- Set dynamics --- # n_shooting = model.get_n_shooting(final_time=final_time) numerical_data_time_series, stim_idx_at_node_list = model.get_numerical_data_time_series(n_shooting, final_time) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds = OcpFes.set_x_bounds(model) diff --git a/examples/other_fes_models/veltink1992_example.py b/examples/other_fes_models/veltink1992_example.py index 11b71923..1f7de026 100644 --- a/examples/other_fes_models/veltink1992_example.py +++ b/examples/other_fes_models/veltink1992_example.py @@ -1,8 +1,4 @@ -from cocofest import ( - VeltinkModelPulseIntensity, - VeltinkRienerModelPulseIntensityWithFatigue, - OcpFes -) +from cocofest import VeltinkModelPulseIntensity, VeltinkRienerModelPulseIntensityWithFatigue, OcpFes from bioptim import ( OptimalControlProgram, @@ -17,8 +13,9 @@ def prepare_ocp(model, final_time, n_shooting, fmax): # --- Set dynamics --- # - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=None, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=None, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) # --- Set initial guesses and bounds for states and controls --- # x_bounds = BoundsList() diff --git a/examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py b/examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py index bb111967..0663a7e9 100644 --- a/examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py +++ b/examples/sensitivity/force_length_velocity/muscle_relationships_comparison.py @@ -6,17 +6,18 @@ import matplotlib.pyplot as plt -from bioptim import (SolutionMerge, - OdeSolver, - ObjectiveList, - ObjectiveFcn, - ParameterList, - OptimalControlProgram, - ControlType, - ConstraintFcn, - ConstraintList, - Node, - ) +from bioptim import ( + SolutionMerge, + OdeSolver, + ObjectiveList, + ObjectiveFcn, + ParameterList, + OptimalControlProgram, + ControlType, + ConstraintFcn, + ConstraintList, + Node, +) from cocofest import DingModelPulseWidthFrequencyWithFatigue, OcpFesMsk, FesMskModel, CustomObjective, OcpFes @@ -28,8 +29,9 @@ def prepare_ocp(model: FesMskModel, final_time: float, msk_info: dict, fixed_pw) numerical_data_time_series, stim_idx_at_node_list = muscle_model.get_numerical_data_time_series( n_shooting, final_time ) - dynamics_options = OcpFes.declare_dynamics_options(numerical_time_series=numerical_data_time_series, - ode_solver=OdeSolver.RK4(n_integration_steps=10)) + dynamics_options = OcpFes.declare_dynamics_options( + numerical_time_series=numerical_data_time_series, ode_solver=OdeSolver.RK4(n_integration_steps=10) + ) x_bounds, x_init = OcpFesMsk.set_x_bounds(model, msk_info) u_bounds, u_init = OcpFesMsk.set_u_bounds(model, msk_info["with_residual_torque"], max_bound=0.0006) @@ -42,8 +44,7 @@ def prepare_ocp(model: FesMskModel, final_time: float, msk_info: dict, fixed_pw) quadratic=True, ) - model = OcpFesMsk.update_model(model, parameters=ParameterList(use_sx=True), - external_force_set=None) + model = OcpFesMsk.update_model(model, parameters=ParameterList(use_sx=True), external_force_set=None) constraints = ConstraintList() constraints.add( @@ -77,14 +78,16 @@ def prepare_ocp(model: FesMskModel, final_time: float, msk_info: dict, fixed_pw) sol_list = [] sol_time = [] -relationship_activation_dict = {"no_relationship": [False, False, False], - "length": [True, False, False], - "velocity":[False, True, False], - "passive_force":[False, False, True], - "length_velocity": [True, True, False], - "length_passive_force": [True, False, True], - "velocity_passive_force":[False, True, True], - "all_relationship": [True, True, True]} +relationship_activation_dict = { + "no_relationship": [False, False, False], + "length": [True, False, False], + "velocity": [False, True, False], + "passive_force": [False, False, True], + "length_velocity": [True, True, False], + "length_passive_force": [True, False, True], + "velocity_passive_force": [False, True, True], + "all_relationship": [True, True, True], +} keys = list(relationship_activation_dict.keys()) for key in keys: @@ -108,7 +111,7 @@ def prepare_ocp(model: FesMskModel, final_time: float, msk_info: dict, fixed_pw) "bound_data": [0], "with_residual_torque": False, }, - fixed_pw=0.00025 + fixed_pw=0.00025, ) sol = ocp.solve() From f4efa647746c1d165f57b7ee616ae87d10e72103 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Mon, 24 Nov 2025 14:31:32 -0500 Subject: [PATCH 4/5] Bioptim installation --- .github/workflows/run_tests_win.yml | 6 +++--- external/bioptim | 2 +- external/bioptim_install_windows.sh | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/run_tests_win.yml b/.github/workflows/run_tests_win.yml index a7a4a31a..c868d29f 100644 --- a/.github/workflows/run_tests_win.yml +++ b/.github/workflows/run_tests_win.yml @@ -40,6 +40,9 @@ jobs: conda info conda list + - name: Install extra dependencies + run: conda install pip pytest-cov black pytest pytest-cov codecov packaging -cconda-forge + - name: Install bioptim on Windows run: | pwd @@ -47,9 +50,6 @@ jobs: ./bioptim_install_windows.sh 4 ${{ env.PREFIX_WINDOWS }} cd .. - - name: Install extra dependencies - run: conda install pytest-cov black pytest pytest-cov codecov packaging -cconda-forge - - name: Run tests with code coverage run: pytest -v --color=yes --cov-report term-missing --cov=cocofest --cov-report=xml:coverage.xml tests/shard${{ matrix.shard }} diff --git a/external/bioptim b/external/bioptim index 108a490e..e23300dd 160000 --- a/external/bioptim +++ b/external/bioptim @@ -1 +1 @@ -Subproject commit 108a490eeae6137f47808f24d1689af1dc8fcfb0 +Subproject commit e23300dd14438922f63fa6c3b3d44747083af79b diff --git a/external/bioptim_install_windows.sh b/external/bioptim_install_windows.sh index c6ae70cc..07dfeb05 100644 --- a/external/bioptim_install_windows.sh +++ b/external/bioptim_install_windows.sh @@ -9,4 +9,4 @@ git submodule update --recursive --init cd bioptim # Installing bioptim -python setup.py install +pip install . From e131cd20e464a27ed68fd885dab01eae816ab6bb Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Mon, 24 Nov 2025 15:33:17 -0500 Subject: [PATCH 5/5] Comment artefacts --- .../models/ding2007/ding2007_with_fatigue.py | 2 +- cocofest/models/hmed2018/hmed2018.py | 6 +- .../models/hmed2018/hmed2018_with_fatigue.py | 16 +- .../fes_multibody/cycling/cost_functions.py | 773 ------------------ 4 files changed, 12 insertions(+), 785 deletions(-) delete mode 100644 examples/fes_multibody/cycling/cost_functions.py diff --git a/cocofest/models/ding2007/ding2007_with_fatigue.py b/cocofest/models/ding2007/ding2007_with_fatigue.py index da40b8c6..595e3950 100644 --- a/cocofest/models/ding2007/ding2007_with_fatigue.py +++ b/cocofest/models/ding2007/ding2007_with_fatigue.py @@ -126,7 +126,7 @@ def system_dynamics( The system dynamics is the function that describes the models. Parameters - # ---------- + ---------- time: MX The system's current node time states: MX diff --git a/cocofest/models/hmed2018/hmed2018.py b/cocofest/models/hmed2018/hmed2018.py index 6dde979c..ab0a6866 100644 --- a/cocofest/models/hmed2018/hmed2018.py +++ b/cocofest/models/hmed2018/hmed2018.py @@ -124,9 +124,9 @@ def system_dynamics( ) -> MX: """ The system dynamics is the function that describes the models. - # - # Parameters - # ---------- + + Parameters + ---------- time: MX The system's current node time states: MX diff --git a/cocofest/models/hmed2018/hmed2018_with_fatigue.py b/cocofest/models/hmed2018/hmed2018_with_fatigue.py index 39810918..964a4e20 100644 --- a/cocofest/models/hmed2018/hmed2018_with_fatigue.py +++ b/cocofest/models/hmed2018/hmed2018_with_fatigue.py @@ -122,22 +122,22 @@ def system_dynamics( numerical_timeseries: MX, ) -> MX: """ - The system dynamics is the function that describes the models. + The system dynamics is the function that describes the models. - Parameters + Parameters ---------- time: MX The system's current node time - states: MX + states: MX The state of the system CN, F, A, Tau1, Km - controls: MX + controls: MX The controls of the system, pulse_intensity - numerical_timeseries: MX + numerical_timeseries: MX The numerical timeseries of the system - Returns - ------- - The value of the derivative of each state dx/dt at the current time t + Returns + ------- + The value of the derivative of each state dx/dt at the current time t """ t = time cn = states[0] diff --git a/examples/fes_multibody/cycling/cost_functions.py b/examples/fes_multibody/cycling/cost_functions.py deleted file mode 100644 index 885a2522..00000000 --- a/examples/fes_multibody/cycling/cost_functions.py +++ /dev/null @@ -1,773 +0,0 @@ -from casadi import MX, vertcat, sum1, mmax, fabs, sign -from bioptim import PenaltyController -from cocofest.models.ding2007.ding2007 import DingModelPulseWidthFrequency - - -class CustomCostFunctions: - def __init__(self): - self.dict_functions = { - "minimize_average_activation": { - "function": self.minimize_average_activation, - "index": 1, - "latex": r"\phi_1 = \frac{1}{n_m}\sum_{j=1}^{n_m} a^{j}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", - "description": "Minimize the average muscle activation", - "power": "1", - "state": "pw", - }, - "minimize_root_mean_square_activation": { - "function": self.minimize_root_mean_square_activation, - "index": 2, - "latex": r"\phi_2 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (a^{j})^{2}\right)^{\tfrac{1}{2}}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", - "description": "Minimize the root mean square of muscle activation", - "power": "2", - "state": "pw", - }, - "minimize_cubic_average_activation": { - "function": self.minimize_cubic_average_activation, - "index": 3, - "latex": r"\phi_3 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (a^{j})^{3}\right)^{\tfrac{1}{3}}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", - "description": "Minimize the cubic average of muscle activation", - "power": "3", - "state": "pw", - }, - "minimize_peak_activation": { - "function": self.minimize_peak_activation, - "index": 4, - "latex": r"\phi_4 = \max_{j=1,\ldots,n_m} \; a^{j}, \quad a^{j}=\frac{f^{j}-f^{j}_{\min}}{f^{j}_{\max}-f^{j}_{\min}}", - "description": "Minimize the peak of muscle activation", - "power": "max", - "state": "pw", - }, - "minimize_average_force": { - "function": self.minimize_average_force, - "index": 5, - "latex": r"\phi_5 = \frac{1}{n_m}\sum_{j=1}^{n_m} f^{j}", - "description": "Minimize the average muscle force", - "power": "1", - "state": "f", - }, - "minimize_root_mean_square_force": { - "function": self.minimize_root_mean_square_force, - "index": 6, - "latex": r"\phi_6 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j})^{2}\right)^{\tfrac{1}{2}}", - "description": "Minimize the root mean square of muscle force", - "power": "2", - "state": "f", - }, - "minimize_cubic_average_force": { - "function": self.minimize_cubic_average_force, - "index": 7, - "latex": r"\phi_7 = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j})^{3}\right)^{\tfrac{1}{3}}", - "description": "Minimize the cubic average of muscle force", - "power": "3", - "state": "f", - }, - "minimize_peak_force": { - "function": self.minimize_peak_force, - "index": 8, - "latex": r"\phi_8 = \max_{j=1,\ldots,n_m} \; f^{j}", - "description": "Minimize the peak muscle force", - "power": "max", - "state": "f", - }, - "minimize_average_muscle_stress": { - "function": self.minimize_average_muscle_stress, - "index": 9, - "latex": r"\phi_9 = \frac{1}{n_m}\sum_{j=1}^{n_m} \frac{f^{j}}{S^{j}}", - "description": "Minimize the average muscle stress", - "power": "1", - "state": "str", - }, - "minimize_root_mean_square_muscle_stress": { - "function": self.minimize_root_mean_square_muscle_stress, - "index": 10, - "latex": r"\phi_{10} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} \left(\frac{f^{j}}{S^{j}}\right)^{2}\right)^{\tfrac{1}{2}}", - "description": "Minimize the root mean square of muscle stress", - "power": "2", - "state": "str", - }, - "minimize_cubic_average_muscle_stress": { - "function": self.minimize_cubic_average_muscle_stress, - "index": 11, - "latex": r"\phi_{11} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} \left(\frac{f^{j}}{S^{j}}\right)^{3}\right)^{\tfrac{1}{3}}", - "description": "Minimize the cubic average of muscle stress", - "power": "3", - "state": "str", - }, - "minimize_peak_muscle_stress": { - "function": self.minimize_peak_muscle_stress, - "index": 12, - "latex": r"\phi_{12} = \max_{j=1,\ldots,n_m} \; \frac{f^{j}}{S^{j}}", - "description": "Minimize the peak muscle stress", - "power": "max", - "state": "str", - }, - "minimize_average_muscle_power": { - "function": self.minimize_average_muscle_power, - "index": 13, - "latex": r"\phi_{13} = \frac{1}{n_m}\sum_{j=1}^{n_m} f^{j} v^{j}", - "description": "Minimize the average muscle power", - "power": "1", - "state": "pow", - }, - "minimize_root_mean_square_muscle_power": { - "function": self.minimize_root_mean_square_muscle_power, - "index": 14, - "latex": r"\phi_{14} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j} v^{j})^{2}\right)^{\tfrac{1}{2}}", - "description": "Minimize the root mean square of muscle power", - "power": "2", - "state": "pow", - }, - "minimize_cubic_average_muscle_power": { - "function": self.minimize_cubic_average_muscle_power, - "index": 15, - "latex": r"\phi_{15} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (f^{j} v^{j})^{3}\right)^{\tfrac{1}{3}}", - "description": "Minimize the cubic average of muscle power", - "power": "3", - "state": "pow", - }, - "minimize_peak_muscle_power": { - "function": self.minimize_peak_muscle_power, - "index": 16, - "latex": r"\phi_{16} = \max_{j=1,\ldots,n_m} \; f^{j} v^{j}}", - "description": "Minimize the peak muscle power", - "power": "max", - "state": "pow", - }, - "minimize_average_fatigue": { - "function": self.minimize_average_fatigue, - "index": 17, - "latex": r"\phi_{17} = \frac{1}{n_m}\sum_{j=1}^{n_m} \mathcal{F}^{j}", - "description": "Minimize the average muscle fatigue", - "power": "1", - "state": "fat", - }, - "minimize_root_mean_square_fatigue": { - "function": self.minimize_root_mean_square_fatigue, - "index": 18, - "latex": r"\phi_{18} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (\mathcal{F}^{j})^{2}\right)^{\tfrac{1}{2}}", - "description": "Minimize the root mean square of muscle fatigue", - "power": "2", - "state": "fat", - }, - "minimize_cubic_average_fatigue": { - "function": self.minimize_cubic_average_fatigue, - "index": 19, - "latex": r"\phi_{19} = \left(\frac{1}{n_m}\sum_{j=1}^{n_m} (\mathcal{F}^{j})^{3}\right)^{\tfrac{1}{3}}", - "description": "Minimize the cubic average of muscle fatigue", - "power": "3", - "state": "fat", - }, - "minimize_peak_fatigue": { - "function": self.minimize_peak_fatigue, - "index": 20, - "latex": r"\phi_{20} = \max_{j=1,\ldots,n_m} \; \mathcal{F}^{j}", - "description": "Minimize the peak muscle fatigue", - "power": "max", - "state": "fat", - }, - "minimize_peak": { - "function": self.minimize_peak, - "index": 99, - "latex": r"\phi_{99} = \max_{j=1,\ldots,n_m} \; \mathcal{Var}^{j}", - "description": "Minimize the peak of a variable", - }, - } - - # --- Electrical stimulation cost functions --- # - @staticmethod - def minimize_average_activation(controller: PenaltyController) -> MX: - """ - Minimize the average muscle activation. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The average of muscle activation - """ - muscle_name_list = controller.model.bio_model.muscle_names - if isinstance(controller.model.muscles_dynamics_model[0], DingModelPulseWidthFrequency): - 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]].min[0][0] - ) - / ( - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] - - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] - ) - for x in range(len(muscle_name_list)) - ] - ) - else: - raise NotImplementedError( - "Minimizing average activation is only implemented for DingModelPulseWidthFrequency." - ) - - return sum1(stim_charge) / len(muscle_name_list) - - @staticmethod - def minimize_root_mean_square_activation(controller: PenaltyController) -> MX: - """ - Minimize the root-mean-square of muscle activation. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The root-mean-square of muscle activation - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - if isinstance(controller.model.muscles_dynamics_model[0], DingModelPulseWidthFrequency): - 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]].min[0][0] - ) - / ( - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] - - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] - ) - ) - ** 2 - for x in range(len(muscle_name_list)) - ] - ) - else: - raise NotImplementedError( - "Minimizing average activation is only implemented for DingModelPulseWidthFrequency." - ) - - rms_activation = (sum1(stim_charge) / len(muscle_name_list) + eps) ** 0.5 - return rms_activation - - @staticmethod - def minimize_cubic_average_activation(controller: PenaltyController) -> MX: - """ - Minimize the cubic average of muscle activation. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The cubic average of muscle activation - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - if isinstance(controller.model.muscles_dynamics_model[0], DingModelPulseWidthFrequency): - 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]].min[0][0] - ) - / ( - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] - - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] - ) - ) - ** 3 - for x in range(len(muscle_name_list)) - ] - ) - else: - raise NotImplementedError( - "Minimizing average activation is only implemented for DingModelPulseWidthFrequency." - ) - - x = sum1(stim_charge) / len(muscle_name_list) - cubic_avg_activation = sign(x) * (fabs(x) + eps) ** (1 / 3) - # cubic_avg_activation = (sum1(stim_charge) / len(muscle_name_list) + eps) ** (1/3) - return cubic_avg_activation - - @staticmethod - def minimize_peak_activation(controller: PenaltyController) -> MX: - """ - Minimize the peak muscle activation. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The peak of muscle activation - """ - muscle_name_list = controller.model.bio_model.muscle_names - stim_activation = vertcat( - *[ - ( - controller.controls["last_pulse_width_" + muscle_name_list[x]].cx - - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] - ) - / ( - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].max[0][0] - - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[x]].min[0][0] - ) - for x in range(len(muscle_name_list)) - ] - ) - max_activation = mmax(stim_activation) - return max_activation - - # --- Muscle force cost functions --- # - @staticmethod - def minimize_average_force(controller: PenaltyController) -> MX: - """ - Minimize the average muscle force production. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The average of produced force - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_force = vertcat( - *[controller.states["F_" + muscle_name_list[x]].cx for x in range(len(muscle_name_list))] - ) - return sum1(muscle_force) / len(muscle_name_list) - - @staticmethod - def minimize_root_mean_square_force(controller: PenaltyController) -> MX: - """ - Minimize the root-mean-square of muscle force production. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The root-mean-square of produced force - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_force = vertcat( - *[controller.states["F_" + muscle_name_list[x]].cx ** 2 for x in range(len(muscle_name_list))] - ) - rms_force = (sum1(muscle_force) / len(muscle_name_list) + eps) ** 0.5 - return rms_force - - @staticmethod - def minimize_cubic_average_force(controller: PenaltyController) -> MX: - """ - Minimize the cubic average of muscle force production. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The cubic average of produced force - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_force = vertcat( - *[controller.states["F_" + muscle_name_list[x]].cx ** 3 for x in range(len(muscle_name_list))] - ) - cubic_avg_force = (sum1(muscle_force) / len(muscle_name_list) + eps) ** (1 / 3) - return cubic_avg_force - - @staticmethod - def minimize_peak_force(controller: PenaltyController) -> MX: - """ - Minimize the peak muscle force production. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The peak of produced force - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_force = vertcat( - *[controller.states["F_" + muscle_name_list[x]].cx for x in range(len(muscle_name_list))] - ) - max_force = mmax(muscle_force) - return max_force - - # --- Muscle stress cost functions --- # - @staticmethod - def minimize_average_muscle_stress(controller: PenaltyController) -> MX: - """ - Minimize the average muscle stress. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The average of muscle stress - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_stress = vertcat( - *[ - controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa - for x in range(len(muscle_name_list)) - ] - ) - return sum1(muscle_stress) / len(muscle_name_list) - - @staticmethod - def minimize_root_mean_square_muscle_stress(controller: PenaltyController) -> MX: - """ - Minimize the root-mean-square of muscle stress. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The root-mean-square of muscle stress - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_stress = vertcat( - *[ - (controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa) - ** 2 - for x in range(len(muscle_name_list)) - ] - ) - rms_stress = (sum1(muscle_stress) / len(muscle_name_list) + eps) ** 0.5 - return rms_stress - - @staticmethod - def minimize_cubic_average_muscle_stress(controller: PenaltyController) -> MX: - """ - Minimize the cubic average of muscle stress. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The cubic average of muscle stress - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_stress = vertcat( - *[ - (controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa) - ** 3 - for x in range(len(muscle_name_list)) - ] - ) - cubic_avg_stress = (sum1(muscle_stress) / len(muscle_name_list) + eps) ** (1 / 3) - return cubic_avg_stress - - @staticmethod - def minimize_peak_muscle_stress(controller: PenaltyController) -> MX: - """ - Minimize the peak muscle stress. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The peak of muscle stress - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_stress = vertcat( - *[ - controller.states["F_" + muscle_name_list[x]].cx / controller.model.muscles_dynamics_model[x].pcsa - for x in range(len(muscle_name_list)) - ] - ) - max_stress = mmax(muscle_stress) - return max_stress - - # --- Muscle power cost functions --- # - @staticmethod - def minimize_average_muscle_power(controller: PenaltyController) -> MX: - """ - Minimize the average of muscle power. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The average of muscle power - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_velocity = controller.model.muscle_velocity()( - controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx - ) - muscle_power = vertcat( - *[ - (controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) - for x in range(len(muscle_name_list)) - ] - ) - return sum1(muscle_power) / len(muscle_name_list) - - @staticmethod - def minimize_root_mean_square_muscle_power(controller: PenaltyController) -> MX: - """ - Minimize the root-mean-square of muscle power. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The root-mean-square of muscle power - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_velocity = controller.model.muscle_velocity()( - controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx - ) - muscle_power = vertcat( - *[ - (controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) ** 2 - for x in range(len(muscle_name_list)) - ] - ) - rms_power = (sum1(muscle_power) / len(muscle_name_list) + eps) ** 0.5 - return rms_power - - @staticmethod - def minimize_cubic_average_muscle_power(controller: PenaltyController) -> MX: - """ - Minimize the cubic average of muscle power. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The cubic average of muscle power - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_velocity = controller.model.muscle_velocity()( - controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx - ) - muscle_power = vertcat( - *[ - ((controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) ** 2 + 1e-6) ** 1.5 - for x in range(len(muscle_name_list)) - ] - ) - cubic_avg_power = ((sum1(muscle_power)) / len(muscle_name_list) + eps) ** (1 / 3) - return cubic_avg_power - - @staticmethod - def minimize_peak_muscle_power(controller: PenaltyController) -> MX: - """ - Minimize the peak muscle power. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The peak of muscle power - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_velocity = controller.model.muscle_velocity()( - controller.states["q"].cx, controller.states["qdot"].cx, controller.parameters.cx - ) - muscle_power = vertcat( - *[ - (controller.states["F_" + muscle_name_list[x]].cx * muscle_velocity[x]) - for x in range(len(muscle_name_list)) - ] - ) - max_power = mmax(muscle_power) - return max_power - - # --- Muscle fatigue cost functions --- # - @staticmethod - def minimize_average_fatigue(controller: PenaltyController) -> MX: - """ - Minimize the average muscle fatigue. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The average of muscle fatigue - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_fatigue = vertcat( - *[ - controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx - for x in range(len(muscle_name_list)) - ] - ) - return sum1(muscle_fatigue) / len(muscle_name_list) - - @staticmethod - def minimize_root_mean_square_fatigue(controller: PenaltyController) -> MX: - """ - Minimize the root-mean-square of muscle fatigue. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The root-mean-square of muscle fatigue - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_fatigue = vertcat( - *[ - (controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx) - ** 2 - for x in range(len(muscle_name_list)) - ] - ) - rms_fatigue = (sum1(muscle_fatigue) / len(muscle_name_list) + eps) ** 0.5 - return rms_fatigue - - @staticmethod - def minimize_cubic_average_fatigue(controller: PenaltyController) -> MX: - """ - Minimize the cubic average of muscle fatigue. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The cubic average of muscle fatigue - """ - eps = 1e-8 - muscle_name_list = controller.model.bio_model.muscle_names - muscle_fatigue = vertcat( - *[ - (controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx) - ** 3 - for x in range(len(muscle_name_list)) - ] - ) - cubic_avg_fatigue = (sum1(muscle_fatigue) / len(muscle_name_list) + eps) ** (1 / 3) - return cubic_avg_fatigue - - @staticmethod - def minimize_peak_fatigue(controller: PenaltyController) -> MX: - """ - Minimize the peak muscle fatigue. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The peak of muscle fatigue - """ - muscle_name_list = controller.model.bio_model.muscle_names - muscle_fatigue = vertcat( - *[ - controller.model.muscles_dynamics_model[x].a_scale - controller.states["A_" + muscle_name_list[x]].cx - for x in range(len(muscle_name_list)) - ] - ) - max_fatigue = mmax(muscle_fatigue) - return max_fatigue - - # --- Peak cost function and constraint used in OCP --- # - @staticmethod - def minimize_peak(controller: PenaltyController) -> MX: - """ - Minimize the peak of a variable. - - Parameters - ---------- - controller: PenaltyController - The penalty node elements - - Returns - ------- - The peak of a variable - """ - return controller.parameters["minmax_param"].cx - - @staticmethod - def constraints_minmax(controller: PenaltyController, obj_fun_key: str, param_index: int) -> MX: - muscle_name_list = controller.model.bio_model.muscle_names - - if obj_fun_key == ["minimize_peak_force"]: - value = controller.states["F_" + muscle_name_list[param_index]].cx - - elif obj_fun_key == ["minimize_peak_activation"]: - value = ( - controller.controls["last_pulse_width_" + muscle_name_list[param_index]].cx - - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[param_index]].min[0][0] - ) / ( - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[param_index]].max[0][0] - - controller.ocp.nlp[0].u_bounds["last_pulse_width_" + muscle_name_list[param_index]].min[0][0] - ) - - elif obj_fun_key == ["minimize_peak_muscle_stress"]: - value = ( - controller.states["F_" + muscle_name_list[param_index]].cx - / controller.model.muscles_dynamics_model[param_index].pcsa - ) - - elif obj_fun_key == ["minimize_peak_fatigue"]: - value = ( - controller.model.muscles_dynamics_model[param_index].a_scale - - controller.states["A_" + muscle_name_list[param_index]].cx - ) - - else: - raise NotImplementedError(f"The cost function {obj_fun_key}, is not implementend in minmax") - - return controller.parameters["minmax_param"].cx[controller.node_index] - value