diff --git a/cocofest/__init__.py b/cocofest/__init__.py index d390212..f985e9b 100644 --- a/cocofest/__init__.py +++ b/cocofest/__init__.py @@ -33,3 +33,5 @@ from .result.plot import PlotCyclingResult from .result.pickle import SolutionToPickle from .result.animate import PickleAnimate +from .integration.integrator import ModifiedOdeSolverRK4 +from .result.graphics import FES_plot \ No newline at end of file diff --git a/cocofest/custom_constraints.py b/cocofest/custom_constraints.py index fdaf5ce..f3e7c64 100644 --- a/cocofest/custom_constraints.py +++ b/cocofest/custom_constraints.py @@ -96,3 +96,7 @@ def a_calculation_identification(controller: PenaltyController, last_stim_index: pdt=pdt, ) return controller.controls["A_calculation"].cx - a_calculation + + @staticmethod + def last_pulse_width(controller: PenaltyController, last_stim_index: int) -> MX | SX: + return controller.parameters["pulse_width"].cx[last_stim_index] - controller.controls["last_pulse_width"].cx diff --git a/cocofest/custom_objectives.py b/cocofest/custom_objectives.py index fc2fefd..b5e91e3 100644 --- a/cocofest/custom_objectives.py +++ b/cocofest/custom_objectives.py @@ -4,7 +4,7 @@ """ import numpy as np -from casadi import MX, SX, sum1, horzcat +from casadi import MX, SX, sum1, horzcat, vertcat from bioptim import PenaltyController from .fourier_approx import FourierSeries @@ -111,7 +111,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_force = horzcat( + muscle_force = vertcat( *[controller.states["F_" + muscle_name_list[x]].cx for x in range(len(muscle_name_list))] ) - return sum1(muscle_force) + return muscle_force diff --git a/cocofest/dynamics/inverse_kinematics_and_dynamics.py b/cocofest/dynamics/inverse_kinematics_and_dynamics.py index 243bc54..c25f177 100644 --- a/cocofest/dynamics/inverse_kinematics_and_dynamics.py +++ b/cocofest/dynamics/inverse_kinematics_and_dynamics.py @@ -1,5 +1,6 @@ import math import numpy as np +from scipy.interpolate import interp1d import biorbd @@ -50,6 +51,7 @@ def inverse_kinematics_cycling( y_center: int | float, radius: int | float, ik_method: str = "trf", + cycling_number: int = 1, ) -> tuple: """ Perform the inverse kinematics of a cycling movement @@ -80,6 +82,8 @@ def inverse_kinematics_cycling( -‘lm’ : Levenberg-Marquardt algorithm as implemented in MINPACK. Does not handle bounds and sparse Jacobians. Usually the most efficient method for small unconstrained problems. + cycling_number: int + The number of cycle performed in a single problem Returns ---------- @@ -88,20 +92,27 @@ def inverse_kinematics_cycling( """ model = biorbd.Model(model_path) - if 0 <= model.nbMarkers() > 1: - raise ValueError("The model must have only one markers to perform the inverse kinematics") - z = model.markers(np.array([0, 0]))[0].to_array()[2] - if z != model.markers(np.array([np.pi / 2, np.pi / 2]))[0].to_array()[2]: + z = model.markers(np.array([0]*model.nbQ()))[0].to_array()[2] + if z != model.markers(np.array([np.pi / 2]*model.nbQ()))[0].to_array()[2]: print("The model not strictly 2d. Warm start not optimal.") + f = interp1d(np.linspace(0, -360 * cycling_number, 360 * cycling_number + 1), + np.linspace(0, -360 * cycling_number, 360 * cycling_number + 1), kind="linear") + x_new = f(np.linspace(0, -360 * cycling_number, n_shooting+1)) + x_new_rad = np.deg2rad(x_new) + x_y_z_coord = np.array( [ - get_circle_coord(theta, x_center, y_center, radius, z=z) - for theta in np.linspace(0, -2 * np.pi + (-2 * np.pi / n_shooting), n_shooting + 1) + get_circle_coord(theta, x_center, y_center, radius) + for theta in x_new_rad ] ).T - target_q = x_y_z_coord.reshape((3, 1, n_shooting + 1)) + + target_q_hand = x_y_z_coord.reshape((3, 1, n_shooting+1)) # Hand marker_target + wheel_center_x_y_z_coord = np.array([x_center, y_center, z]) + target_q_wheel_center = np.tile(wheel_center_x_y_z_coord[:, np.newaxis, np.newaxis], (1, 1, n_shooting+1)) # Wheel marker_target + target_q = np.concatenate((target_q_hand, target_q_wheel_center), axis=1) ik = biorbd.InverseKinematics(model, target_q) ik_q = ik.solve(method=ik_method) ik_qdot = np.array([np.gradient(ik_q[i], (1 / n_shooting)) for i in range(ik_q.shape[0])]) @@ -136,9 +147,9 @@ def inverse_dynamics_cycling( joints torques """ model = biorbd.Model(model_path) - tau_shape = (model.nbQ(), q.shape[1] - 1) + tau_shape = (model.nbQ(), q.shape[1]) tau = np.zeros(tau_shape) for i in range(tau.shape[1]): tau_i = model.InverseDynamics(q[:, i], qdot[:, i], qddot[:, i]) tau[:, i] = tau_i.to_array() - return tau + return tau[:, :-1] diff --git a/cocofest/examples/dynamics/cycling/cycling_fes_driven.py b/cocofest/examples/dynamics/cycling/cycling_fes_driven.py deleted file mode 100644 index 107452e..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_fes_driven.py +++ /dev/null @@ -1,71 +0,0 @@ -""" -This example will do an optimal control program of a 10 stimulation example with Ding's 2007 pulse width model. -Those ocp were build to produce a cycling motion. -The stimulation frequency will be set to 10Hz and pulse width will be optimized to satisfy the motion meanwhile -reducing residual torque. -""" - -import numpy as np - -from bioptim import CostType, Solver - -import biorbd - -from cocofest import ( - DingModelPulseWidthFrequencyWithFatigue, - OcpFesMsk, - PlotCyclingResult, - SolutionToPickle, - FesMskModel, - PickleAnimate, -) - - -def main(): - minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 - - model = FesMskModel( - name=None, - biorbd_path="../../msk_models/simplified_UL_Seth.bioMod", - muscles_model=[ - DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A"), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusScapula_P"), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="TRIlong"), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_long"), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_brevis"), - ], - activate_force_length_relationship=True, - activate_force_velocity_relationship=True, - activate_residual_torque=True, - ) - - ocp = OcpFesMsk.prepare_ocp( - model=model, - stim_time=list(np.round(np.linspace(0, 1, 11), 3))[:-1], - final_time=1, - pulse_width={ - "min": minimum_pulse_width, - "max": 0.0006, - "bimapping": False, - }, - msk_info={"with_residual_torque": True}, - objective={ - "cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1, "target": "marker"}, - "minimize_residual_torque": True, - }, - initial_guess_warm_start=False, - n_threads=5, - ) - ocp.add_plot_penalty(CostType.ALL) - sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) - SolutionToPickle(sol, "cycling_fes_driven_min_residual_torque.pkl", "").pickle() - - biorbd_model = biorbd.Model("../../msk_models/simplified_UL_Seth_full_mesh.bioMod") - PickleAnimate("cycling_fes_driven_min_residual_torque.pkl").animate(model=biorbd_model) - - sol.graphs(show_bounds=False) - PlotCyclingResult(sol).plot(starting_location="E") - - -if __name__ == "__main__": - main() diff --git a/cocofest/examples/dynamics/cycling/cycling_inverse_kinematics.py b/cocofest/examples/dynamics/cycling/cycling_inverse_kinematics.py new file mode 100644 index 0000000..79485be --- /dev/null +++ b/cocofest/examples/dynamics/cycling/cycling_inverse_kinematics.py @@ -0,0 +1,91 @@ +""" +This example will do an inverse kinematics and dynamics of a 100 steps hand cycling motion. +""" + +import numpy as np +import matplotlib.pyplot as plt +from scipy.interpolate import interp1d + +import biorbd +from pyorerun import BiorbdModel, PhaseRerun + +from cocofest import get_circle_coord + + +def main(show_plot=True, animate=True): + n_shooting = 1000 + cycling_number = 1 + + # Define the circle parameters + x_center = 0.35 + y_center = 0 + radius = 0.1 + + # Load a predefined model + model = biorbd.Model("../../msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod") + + # Define the marker target to match + z = model.markers(np.array([0] * model.nbQ()))[0].to_array()[2] + if z != model.markers(np.array([np.pi / 2] * model.nbQ()))[0].to_array()[2]: + print("The model not strictly 2d. Warm start not optimal.") + + f = interp1d(np.linspace(0, -360 * cycling_number, 360 * cycling_number + 1), + np.linspace(0, -360 * cycling_number, 360 * cycling_number + 1), kind="linear") + x_new = f(np.linspace(0, -360 * cycling_number, n_shooting + 1)) + x_new_rad = np.deg2rad(x_new) + + x_y_z_coord = np.array( + [ + get_circle_coord(theta, x_center, y_center, radius) + for theta in x_new_rad + ] + ).T + + target_q_hand = x_y_z_coord.reshape((3, 1, n_shooting + 1)) # Hand marker_target + wheel_center_x_y_z_coord = np.array([x_center, y_center, z]) + target_q_wheel_center = np.tile(wheel_center_x_y_z_coord[:, np.newaxis, np.newaxis], + (1, 1, n_shooting + 1)) # Wheel marker_target + target_q = np.concatenate((target_q_hand, target_q_wheel_center), axis=1) + + # Perform the inverse kinematics + ik = biorbd.InverseKinematics(model, target_q) + ik_q = ik.solve(method="lm") + ik_qdot = np.array([np.gradient(ik_q[i], (1 / n_shooting)) for i in range(ik_q.shape[0])]) + ik_qddot = np.array([np.gradient(ik_qdot[i], (1 / n_shooting)) for i in range(ik_qdot.shape[0])]) + + # Perform the inverse dynamics + tau_shape = (model.nbQ(), ik_q.shape[1] - 1) + tau = np.zeros(tau_shape) + for i in range(tau.shape[1]): + tau_i = model.InverseDynamics(ik_q[:, i], ik_qdot[:, i], ik_qddot[:, i]) + tau[:, i] = tau_i.to_array() + + # Plot the results + if show_plot: + fig, (ax1, ax2) = plt.subplots(1, 2) + ax1.set_title("Q") + ax1.plot(np.linspace(0, 1, n_shooting + 1), ik_q[0], color="orange", label="shoulder") + ax1.plot(np.linspace(0, 1, n_shooting + 1), ik_q[1], color="blue", label="elbow") + ax1.set(xlabel="Time (s)", ylabel="Angle (rad)") + ax2.set_title("Tau") + ax2.plot(np.linspace(0, 1, n_shooting)[2:-1], tau[0][2:-1], color="orange", label="shoulder") + ax2.plot(np.linspace(0, 1, n_shooting)[2:-1], tau[1][2:-1], color="blue", label="elbow") + ax2.set(xlabel="Time (s)", ylabel="Torque (N.m)") + plt.legend() + plt.show() + + # pyorerun animation + if animate: + biorbd_model = biorbd.Model("../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod") + prr_model = BiorbdModel.from_biorbd_object(biorbd_model) + + nb_seconds = 1 + t_span = np.linspace(0, nb_seconds, n_shooting+1) + + viz = PhaseRerun(t_span) + viz.add_animated_model(prr_model, ik_q) + viz.rerun("msk_model") + + +if __name__ == "__main__": + main(show_plot=True, animate=True) diff --git a/cocofest/examples/dynamics/cycling/cycling_muscle_driven.py b/cocofest/examples/dynamics/cycling/cycling_muscle_driven.py deleted file mode 100644 index 19bbd7c..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_muscle_driven.py +++ /dev/null @@ -1,124 +0,0 @@ -""" -This example will do an optimal control program of a 100 steps tracking a hand cycling motion with a muscle driven control. -""" - -import numpy as np - -from bioptim import ( - Axis, - BiorbdModel, - BoundsList, - DynamicsFcn, - DynamicsList, - InitialGuessList, - InterpolationType, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - OptimalControlProgram, - PhaseDynamics, - Solver, - Node, - CostType, -) - -from cocofest import get_circle_coord, inverse_kinematics_cycling - - -def prepare_ocp( - biorbd_model_path: str, - n_shooting: int, - final_time: int, - objective: dict, - initial_guess_warm_start: bool = False, -) -> OptimalControlProgram: - - # Adding the model - bio_model = BiorbdModel( - biorbd_model_path, - ) - - # Adding an objective function to track a marker in a circular trajectory - x_center = objective["cycling"]["x_center"] - y_center = objective["cycling"]["y_center"] - radius = objective["cycling"]["radius"] - circle_coord_list = np.array( - [ - get_circle_coord(theta, x_center, y_center, radius)[:-1] - for theta in np.linspace(0, -2 * np.pi, n_shooting + 1) - ] - ).T - objective_functions = ObjectiveList() - objective_functions.add( - ObjectiveFcn.Mayer.TRACK_MARKERS, - weight=100, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list, - node=Node.ALL, - phase=0, - quadratic=True, - ) - - # Dynamics - dynamics = DynamicsList() - dynamics.add( - DynamicsFcn.MUSCLE_DRIVEN, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - ) - - # Path constraint - x_bounds = BoundsList() - q_x_bounds = bio_model.bounds_from_ranges("q") - qdot_x_bounds = bio_model.bounds_from_ranges("qdot") - x_bounds.add(key="q", bounds=q_x_bounds, phase=0) - x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) - - # Define control path constraint - u_bounds = BoundsList() - u_bounds["muscles"] = [0] * bio_model.nb_muscles, [1] * bio_model.nb_muscles - - # Initial q guess - x_init = InitialGuessList() - u_init = InitialGuessList() - # If warm start, the initial guess is the result of the inverse kinematics - if initial_guess_warm_start: - q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( - biorbd_model_path, n_shooting, x_center, y_center, radius, ik_method="trf" - ) - x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) - x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) - - return OptimalControlProgram( - bio_model, - dynamics, - n_shooting, - final_time, - x_bounds=x_bounds, - u_bounds=u_bounds, - x_init=x_init, - u_init=u_init, - objective_functions=objective_functions, - ode_solver=OdeSolver.RK4(), - n_threads=8, - ) - - -def main(): - # --- Prepare the ocp --- # - ocp = prepare_ocp( - biorbd_model_path="../../msk_models/simplified_UL_Seth.bioMod", - n_shooting=100, - final_time=1, - objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}}, - initial_guess_warm_start=True, - ) - ocp.add_plot_penalty(CostType.ALL) - sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) - sol.animate() - sol.graphs() - - -if __name__ == "__main__": - main() diff --git a/cocofest/examples/dynamics/cycling/cycling_torque_driven.py b/cocofest/examples/dynamics/cycling/cycling_torque_driven.py deleted file mode 100644 index 3087710..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_torque_driven.py +++ /dev/null @@ -1,127 +0,0 @@ -""" -This example will do an optimal control program of a 100 steps tracking a hand cycling motion with a torque driven -control. -""" - -import numpy as np - -from bioptim import ( - Axis, - BiorbdModel, - BoundsList, - CostType, - DynamicsFcn, - DynamicsList, - InitialGuessList, - InterpolationType, - Node, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - OptimalControlProgram, - PhaseDynamics, -) - -from cocofest import ( - get_circle_coord, - inverse_kinematics_cycling, - inverse_dynamics_cycling, -) - - -def prepare_ocp( - biorbd_model_path: str, - n_shooting: int, - final_time: int, - objective: dict, - initial_guess_warm_start: bool = False, -) -> OptimalControlProgram: - - # Adding the model - bio_model = BiorbdModel( - biorbd_model_path, - ) - - # Adding an objective function to track a marker in a circular trajectory - x_center = objective["cycling"]["x_center"] - y_center = objective["cycling"]["y_center"] - radius = objective["cycling"]["radius"] - circle_coord_list = np.array( - [get_circle_coord(theta, x_center, y_center, radius)[:-1] for theta in np.linspace(0, -2 * np.pi, n_shooting)] - ).T - objective_functions = ObjectiveList() - objective_functions.add( - ObjectiveFcn.Mayer.TRACK_MARKERS, - weight=100, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list, - node=Node.ALL_SHOOTING, - phase=0, - quadratic=True, - ) - - # Dynamics - dynamics = DynamicsList() - dynamics.add( - DynamicsFcn.TORQUE_DRIVEN, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - ) - - # Path constraint - x_bounds = BoundsList() - q_x_bounds = bio_model.bounds_from_ranges("q") - qdot_x_bounds = bio_model.bounds_from_ranges("qdot") - x_bounds.add(key="q", bounds=q_x_bounds, phase=0) - x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) - - # Define control path constraint - u_bounds = BoundsList() - u_bounds.add(key="tau", min_bound=np.array([-50, -50]), max_bound=np.array([50, 50]), phase=0) - - # Initial q guess - x_init = InitialGuessList() - u_init = InitialGuessList() - # If warm start, the initial guess is the result of the inverse kinematics and dynamics - if initial_guess_warm_start: - q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( - biorbd_model_path, n_shooting, x_center, y_center, radius, ik_method="trf" - ) - x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) - x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) - u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) - u_init.add("tau", u_guess, interpolation=InterpolationType.EACH_FRAME) - - return OptimalControlProgram( - bio_model, - dynamics, - n_shooting, - final_time, - x_bounds=x_bounds, - u_bounds=u_bounds, - x_init=x_init, - u_init=u_init, - objective_functions=objective_functions, - ode_solver=OdeSolver.RK4(), - n_threads=8, - ) - - -def main(): - # --- Prepare the ocp --- # - ocp = prepare_ocp( - biorbd_model_path="../../msk_models/simplified_UL_Seth.bioMod", - n_shooting=100, - final_time=1, - objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}}, - initial_guess_warm_start=True, - ) - ocp.add_plot_penalty(CostType.ALL) - sol = ocp.solve() - sol.animate() - sol.graphs() - - -if __name__ == "__main__": - main() diff --git a/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py new file mode 100644 index 0000000..c11f0ba --- /dev/null +++ b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py @@ -0,0 +1,532 @@ +""" +This example will do an optimal control program of a 100 steps hand cycling motion with either a torque driven / +muscle driven / FES driven dynamics and includes a resistive torque at the handle. +""" + +import numpy as np + +from bioptim import ( + Axis, + BiorbdModel, + BoundsList, + ConstraintList, + ConstraintFcn, + CostType, + DynamicsFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InitialGuessList, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + ParameterList, + PhaseDynamics, + Solver, +) + +from cocofest import ( + CustomObjective, + DingModelPulseWidthFrequencyWithFatigue, + FesMskModel, + inverse_kinematics_cycling, + OcpFesMsk, +) + + +def prepare_ocp( + model: BiorbdModel | FesMskModel, + n_shooting: int, + final_time: int, + turn_number: int, + pedal_config: dict, + pulse_width: dict, + dynamics_type: str = "torque_driven", + use_sx: bool = True, + integration_step: int = 1, +) -> OptimalControlProgram: + """ + Prepare the optimal control program (OCP) with the provided configuration. + + Parameters + ---------- + model: BiorbdModel | FesMskModel + The biomechanical model. + n_shooting: int + Number of shooting nodes. + final_time: int + Total time of the motion. + turn_number: int + Number of complete turns. + pedal_config: dict + Dictionary with pedal configuration (e.g., center and radius). + pulse_width: dict + Dictionary with pulse width parameters for FES-driven dynamics. + dynamics_type: str + Type of dynamics ("torque_driven", "muscle_driven", or "fes_driven"). + use_sx: bool + Whether to use CasADi SX for symbolic computations. + integration_step: int + Integration step for the ODE solver. + + Returns + ------- + An OptimalControlProgram instance configured for the problem. + """ + # Set external forces (e.g., resistive torque at the handle) + numerical_time_series, external_force_set = set_external_forces(n_shooting, torque=-1) + # Set dynamics based on the chosen dynamics type + dynamics = set_dynamics(model, numerical_time_series, dynamics_type_str=dynamics_type) + # Configure objective functions + objective_functions = set_objective_functions(model, dynamics_type) + # Set initial guess for state variables + x_init = set_x_init(n_shooting, pedal_config, turn_number) + # Define state bounds + x_bounds = set_state_bounds( + model=model, + x_init=x_init, + n_shooting=n_shooting, + turn_number=turn_number, + interpolation_type=InterpolationType.EACH_FRAME, + cardinal=1 + ) + # Define control bounds and initial guess + u_init, u_bounds = set_u_bounds_and_init(model, dynamics_type_str=dynamics_type) + # Set constraints + constraints = set_constraints(model, n_shooting, turn_number) + + # Configure FES parameters if using an FES model and pulse_width is provided + parameters = None + parameters_bounds = None + parameters_init = None + parameter_objectives = None + if isinstance(model, FesMskModel) and isinstance(pulse_width, dict): + (parameters, + parameters_bounds, + parameters_init, + parameter_objectives, + ) = OcpFesMsk._build_parameters( + model=model, + pulse_width=pulse_width, + pulse_intensity=None, + use_sx=use_sx, + ) + + # Update the model with external forces and parameters + model = update_model(model, external_force_set, parameters) + + return OptimalControlProgram( + [model], + dynamics, + n_shooting, + final_time, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + ode_solver=OdeSolver.RK1(n_integration_steps=integration_step), + n_threads=20, + constraints=constraints, + parameters=parameters, + parameter_bounds=parameters_bounds, + parameter_init=parameters_init, + parameter_objectives=parameter_objectives, + use_sx=use_sx, + ) + + +def set_external_forces(n_shooting: int, torque: int | float) -> tuple[dict, ExternalForceSetTimeSeries]: + """ + Create an external force time series applying a constant torque. + + Parameters + ---------- + n_shooting: int + Number of shooting nodes. + torque: int | float + Torque value to be applied. + + Returns + ------- + A tuple with a numerical time series dictionary and the ExternalForceSetTimeSeries object. + """ + external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) + external_force_array = np.array([0, 0, torque]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, n_shooting)) + external_force_set.add_torque(segment="wheel", values=reshape_values_array) + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + return numerical_time_series, external_force_set + + +def update_model(model: BiorbdModel | FesMskModel, external_force_set: ExternalForceSetTimeSeries, parameters: ParameterList = None) -> BiorbdModel | FesMskModel: + """ + Update the model with external forces and parameters if necessary. + + Parameters + ---------- + model: BiorbdModel | FesMskModel + The initial model. + external_force_set: ExternalForceSetTimeSeries + The external forces to be applied. + parameters: ParameterList + Optional parameters for the FES model. + + Returns + ------- + Updated model instance. + """ + if isinstance(model, FesMskModel): + model = FesMskModel( + name=model.name, + biorbd_path=model.biorbd_path, + muscles_model=model.muscles_dynamics_model, + stim_time=model.muscles_dynamics_model[0].stim_time, + 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_residual_torque=model.activate_residual_torque, + parameters=parameters, + external_force_set=external_force_set, + ) + else: + model = BiorbdModel(model.path, external_force_set=external_force_set) + + return model + + +def set_dynamics(model: BiorbdModel | FesMskModel, numerical_time_series: dict, dynamics_type_str: str="torque_driven")-> 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, + with_contact=True, + phase=0, + ) + return dynamics + + +def set_objective_functions(model: BiorbdModel | FesMskModel, dynamics_type: str) -> ObjectiveList: + """ + Configure the objective functions for the optimal control problem. + + Parameters + ---------- + model: BiorbdModel | FesMskModel + The biomechanical model. + dynamics_type: str + The type of dynamics used. + + Returns + ------- + An ObjectiveList with the appropriate objectives. + """ + objective_functions = ObjectiveList() + if isinstance(model, FesMskModel): + objective_functions.add( + CustomObjective.minimize_overall_muscle_force_production, + custom_type=ObjectiveFcn.Lagrange, + weight=1, + quadratic=True) + # Uncomment these following lines if muscle fatigue minimization is desired: + # objective_functions.add( + # CustomObjective.minimize_overall_muscle_fatigue, + # custom_type=ObjectiveFcn.Lagrange, + # weight=1, + # quadratic=True) + else: + control_key = "tau" if dynamics_type == "torque_driven" else "muscles" + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key=control_key, weight=1000, quadratic=True) + return objective_functions + + +def set_x_init(n_shooting: int, pedal_config: dict, turn_number: int) -> InitialGuessList: + """ + Set the initial guess for the state variables based on inverse kinematics. + + Parameters + ---------- + n_shooting: int + Number of shooting nodes. + pedal_config: dict + Dictionary with keys "x_center", "y_center", and "radius". + turn_number: int + Number of complete turns. + + Returns + ------- + An InitialGuessList for the state variables. + """ + x_init = InitialGuessList() + # Path to the biomechanical model used for inverse kinematics + biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod" + q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( + biorbd_model_path, + n_shooting, + x_center=pedal_config["x_center"], + y_center=pedal_config["y_center"], + radius=pedal_config["radius"], + ik_method="lm", + cycling_number=turn_number + ) + x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) + # Optionally, add qdot initialization if needed: + # x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) + + # Optional, method to get control initial guess: + # u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) + # u_init.add("tau", u_guess, interpolation=InterpolationType.EACH_FRAME) + + return x_init + + +def set_u_bounds_and_init(bio_model: BiorbdModel | FesMskModel, dynamics_type_str: str) -> tuple[InitialGuessList, BoundsList]: + """ + Define the control bounds and initial guess for the optimal control problem. + + Parameters + ---------- + model: BiorbdModel | FesMskModel + The biomechanical model. + dynamics_type_str: str + Type of dynamics ("torque_driven" or "muscle_driven"). + + Returns + ------- + A tuple containing the initial guess list for controls and the bounds list. + """ + u_bounds = BoundsList() + u_init = InitialGuessList() + if dynamics_type_str == "torque_driven": + u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0) + elif dynamics_type_str == "muscle_driven": + muscle_min, muscle_max, muscle_init = 0.0, 1.0, 0.5 + u_bounds.add(key="muscles", + min_bound=np.array([muscle_min] * bio_model.nb_muscles), + max_bound=np.array([muscle_max] * bio_model.nb_muscles), + phase=0) + u_init.add(key="muscles", + initial_guess=np.array([muscle_init] * bio_model.nb_muscles), + phase=0) + + # For FES dynamic, u_bounds and u_init remains empty + return u_init, u_bounds + + +def set_state_bounds(model: BiorbdModel | FesMskModel, x_init: InitialGuessList, n_shooting: int, turn_number: int, interpolation_type: InterpolationType = InterpolationType.CONSTANT, cardinal: int=4)-> BoundsList: + """ + Set the bounds for the state variables. + + Parameters + ---------- + model: BiorbdModel | FesMskModel + The biomechanical model. + x_init: InitialGuessList + Initial guess for states. + n_shooting: int + Number of shooting nodes. + turn_number: int + Number of complete turns. + interpolation_type: InterpolationType + Interpolation type for the bounds. + cardinal: int + Number of cardinal nodes per turn for bounds adjustment. + + Returns + ------- + A BoundsList object with the defined state bounds. + """ + x_bounds = BoundsList() + # For FES models, retrieve custom bounds + if isinstance(model, FesMskModel): + x_bounds, _ = OcpFesMsk._set_bounds_fes(model) + + # Retrieve default bounds from the model for positions and velocities + q_x_bounds = model.bounds_from_ranges("q") + qdot_x_bounds = model.bounds_from_ranges("qdot") + + if interpolation_type == InterpolationType.EACH_FRAME: + # Replicate bounds for each shooting node + x_min_bound = [] + x_max_bound = [] + for i in range(q_x_bounds.min.shape[0]): + x_min_bound.append([q_x_bounds.min[i][0]] * (n_shooting + 1)) + x_max_bound.append([q_x_bounds.max[i][0]] * (n_shooting + 1)) + + # Adjust bounds at cardinal nodes for a specific coordinate (e.g., index 2) + cardinal_node_list = [i * int(n_shooting / ((n_shooting / (n_shooting / turn_number)) * cardinal)) for i in + range(int((n_shooting / (n_shooting / turn_number)) * cardinal + 1))] + slack = 10 * (np.pi / 180) + for i in range(len(x_min_bound[0])): + x_min_bound[0][i] = 0 + x_max_bound[0][i] = 1 + x_min_bound[1][i] = 1 + x_min_bound[2][i] = x_init["q"].init[2][-1] + x_max_bound[2][i] = x_init["q"].init[2][0] + for i in range(len(cardinal_node_list)): + cardinal_index = cardinal_node_list[i] + x_min_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] if i % cardinal == 0 else \ + x_init["q"].init[2][cardinal_index] - slack + x_max_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] if i % cardinal == 0 else \ + x_init["q"].init[2][cardinal_index] + slack + # x_min_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] - slack + # x_max_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] + slack + + x_bounds.add(key="q", min_bound=x_min_bound, max_bound=x_max_bound, phase=0, + interpolation=InterpolationType.EACH_FRAME) + + else: + x_bounds.add(key="q", bounds=q_x_bounds, phase=0) + + # Modify bounds for velocities (e.g., setting maximum pedal speed to 0 to prevent the pedal to go backward) + qdot_x_bounds.max[0] = [10, 10, 10] + qdot_x_bounds.min[0] = [-10, -10, -10] + qdot_x_bounds.max[1] = [10, 10, 10] + qdot_x_bounds.min[1] = [-10, -10, -10] + qdot_x_bounds.max[2] = [-1, -1, -1] + qdot_x_bounds.min[2] = [-12, -12, -12] + x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) + return x_bounds + + +def set_constraints(model: BiorbdModel | FesMskModel, n_shooting: int, turn_number: int) -> ConstraintList: + """ + Set constraints for the optimal control problem. + + Parameters + ---------- + model: BiorbdModel | FesMskModel + The biomechanical model. + n_shooting: int + Number of shooting nodes. + turn_number: int + Number of complete turns. + + Returns + ------- + A ConstraintList with the defined constraints. + """ + constraints = ConstraintList() + # constraints.add( + # ConstraintFcn.TRACK_MARKERS_VELOCITY, + # node=Node.START, + # marker_index=model.marker_index("wheel_center"), + # axes=[Axis.X, Axis.Y], + # ) + + superimpose_marker_list = [i * int(n_shooting / ((n_shooting / (n_shooting / turn_number)) * 1)) for i in + range(int((n_shooting / (n_shooting / turn_number)) * 1 + 1))] + for i in superimpose_marker_list: + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=i, + axes=[Axis.X, Axis.Y], + ) + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=i, + marker_index=model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + ) + + return constraints + + +def main(): + """ + Main function to configure and solve the optimal control problem. + """ + # --- Configuration --- # + dynamics_type = "fes_driven" # Available options: "torque_driven", "muscle_driven", "fes_driven" + model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod" + pulse_width = None + n_shooting = 300 + final_time = 3 + turn_number = 3 + pedal_config = {"x_center": 0.35, "y_center": 0.0, "radius": 0.1} + + # --- Load the appropriate model --- # + if dynamics_type in ["torque_driven", "muscle_driven"]: + model = BiorbdModel(model_path) + integration_step = 1 + elif dynamics_type == "fes_driven": + # Define muscle dynamics for the FES-driven model + muscles_model = [ + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusScapula_P", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="TRIlong", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_long", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_brevis", is_approximated=False, sum_stim_truncation=10), + ] + stim_time = list(np.linspace(0, 3, 100)[:-1]) + model = FesMskModel( + name=None, + biorbd_path=model_path, + muscles_model=muscles_model, + stim_time=stim_time, + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=False, + external_force_set=None, # External forces will be added later + ) + pulse_width = {"min": DingModelPulseWidthFrequencyWithFatigue().pd0, "max": 0.0006, "bimapping": False, "same_for_all_muscles": False, + "fixed": False} + # Adjust n_shooting based on the stimulation time + n_shooting = len(stim_time) + integration_step = 3 + else: + raise ValueError(f"Dynamics type '{dynamics_type}' not recognized") + + ocp = prepare_ocp( + model=model, + n_shooting=n_shooting, + final_time=final_time, + turn_number=turn_number, + pedal_config=pedal_config, + pulse_width=pulse_width, + dynamics_type=dynamics_type, + use_sx=False, + integration_step=integration_step, + ) + # Add the penalty cost function plot + ocp.add_plot_penalty(CostType.ALL) + # Solve the optimal control problem + sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=100000)) + # Display graphs and animate the solution + sol.graphs(show_bounds=True) + sol.animate(viewer="pyorerun") + + +if __name__ == "__main__": + main() diff --git a/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods_in_nmpc.py b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods_in_nmpc.py new file mode 100644 index 0000000..c0b760c --- /dev/null +++ b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods_in_nmpc.py @@ -0,0 +1,418 @@ +""" +This example will do an optimal control program of a 100 steps tracking a hand cycling motion with a torque driven and +a torque resistance at the handle. +""" +import matplotlib.pyplot as plt +import numpy as np + +from bioptim import ( + Axis, + BiorbdModel, + BoundsList, + ConstraintList, + ConstraintFcn, + CostType, + DynamicsFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InitialGuessList, + InterpolationType, + MultiCyclicCycleSolutions, + MultiCyclicNonlinearModelPredictiveControl, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + PhaseDynamics, + SolutionMerge, + Solution, + Solver, +) + +from cocofest import ( + CustomObjective, + DingModelPulseWidthFrequency, + DingModelPulseWidthFrequencyWithFatigue, + FesMskModel, + inverse_kinematics_cycling, + OcpFesMsk, + + +) + + +class MyCyclicNMPC(MultiCyclicNonlinearModelPredictiveControl): + def advance_window_bounds_states(self, sol, n_cycles_simultaneous=None, **extra): + min_pedal_bound = self.nlp[0].x_bounds["q"].min[-1, 0] + max_pedal_bound = self.nlp[0].x_bounds["q"].max[-1, 0] + super(MyCyclicNMPC, self).advance_window_bounds_states(sol) + self.nlp[0].x_bounds["q"].min[-1, 0] = min_pedal_bound + self.nlp[0].x_bounds["q"].max[-1, 0] = max_pedal_bound + + if sol.parameters != {}: + self.update_stim(sol) + return True + + def advance_window_initial_guess_states(self, sol, n_cycles_simultaneous=None): + # Reimplementation of the advance_window method so the rotation of the wheel restart at -pi + super(MyCyclicNMPC, self).advance_window_initial_guess_states(sol) + q = sol.decision_states(to_merge=SolutionMerge.NODES)["q"] + self.nlp[0].x_init["q"].init[:, :] = q[:, :] # Keep the previously found value for the wheel + return True + + def update_stim(self, sol): + truncation_term = self.nlp[0].model.muscles_dynamics_model[0]._sum_stim_truncation + solution_stimulation_time = self.nlp[0].model.muscles_dynamics_model[0].stim_time[-truncation_term:] + previous_stim_time = [x - self.phase_time[0] for x in solution_stimulation_time] + for i in range(len(self.nlp[0].model.muscles_dynamics_model)): + self.nlp[0].model.muscles_dynamics_model[i].previous_stim = {"time": previous_stim_time} + if isinstance(self.nlp[0].model.muscles_dynamics_model[i], DingModelPulseWidthFrequency): + self.nlp[0].model.muscles_dynamics_model[i].previous_stim["pulse_width"] = list(sol.parameters["pulse_width_" + self.nlp[0].model.muscles_dynamics_model[i].muscle_name][-10:]) + self.nlp[0].model.muscles_dynamics_model[i].all_stim = self.nlp[0].model.muscles_dynamics_model[i].previous_stim["time"] + self.nlp[0].model.muscles_dynamics_model[i].stim_time + + +def prepare_nmpc( + model: BiorbdModel | FesMskModel, + cycle_duration: int | float, + cycle_len: int, + n_cycles_to_advance: int, + n_cycles_simultaneous: int, + total_n_cycles: int, + turn_number: int, + pedal_config: dict, + pulse_width: dict, + dynamics_type: str = "torque_driven", + use_sx: bool = True, +): + + total_n_shooting = cycle_len * n_cycles_simultaneous + + # Dynamics + total_external_forces_frame = total_n_cycles * cycle_len if total_n_cycles >= n_cycles_simultaneous else total_n_shooting + numerical_time_series, external_force_set = set_external_forces(total_external_forces_frame, torque=-1) + dynamics = set_dynamics(model=model, numerical_time_series=numerical_time_series, dynamics_type_str=dynamics_type) + + # Define objective functions + objective_functions = set_objective_functions(model, dynamics_type) + + # Initial q guess + x_init = set_x_init(total_n_shooting, pedal_config, turn_number) + + # Path constraint + x_bounds = set_bounds(model=model, + x_init=x_init, + n_shooting=total_n_shooting, + turn_number=turn_number, + interpolation_type=InterpolationType.EACH_FRAME, + cardinal=1) + + # Control path constraint + u_bounds, u_init = set_u_bounds_and_init(model, dynamics_type_str=dynamics_type) + + # Constraints + constraints = set_constraints(model, total_n_shooting, turn_number) + + # Parameters + parameters = None + parameters_bounds = None + parameters_init = None + parameter_objectives = None + if isinstance(model, FesMskModel) and isinstance(pulse_width, dict): + (parameters, + parameters_bounds, + parameters_init, + parameter_objectives, + ) = OcpFesMsk._build_parameters( + model=model, + pulse_width=pulse_width, + pulse_intensity=None, + use_sx=use_sx, + ) + + # Update model + model = updating_model(model=model, external_force_set=external_force_set, parameters=parameters) + + return MyCyclicNMPC( + [model], + dynamics, + cycle_len=cycle_len, + cycle_duration=cycle_duration, + n_cycles_simultaneous=n_cycles_simultaneous, + n_cycles_to_advance=n_cycles_to_advance, + common_objective_functions=objective_functions, + constraints=constraints, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + parameters=parameters, + parameter_bounds=parameters_bounds, + parameter_init=parameters_init, + parameter_objectives=parameter_objectives, + ode_solver=OdeSolver.RK1(n_integration_steps=1), + n_threads=20, + use_sx=False, + ) + + +def set_external_forces(n_shooting, torque): + external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) + external_force_array = np.array([0, 0, torque]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, n_shooting)) + external_force_set.add_torque(segment="wheel", values=reshape_values_array) + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + return numerical_time_series, external_force_set + + +def updating_model(model, external_force_set, parameters=None): + if isinstance(model, FesMskModel): + model = FesMskModel( + name=model.name, + biorbd_path=model.biorbd_path, + muscles_model=model.muscles_dynamics_model, + stim_time=model.muscles_dynamics_model[0].stim_time, + 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_residual_torque=model.activate_residual_torque, + parameters=parameters, + external_force_set=external_force_set, + ) + else: + model = BiorbdModel(model.path, external_force_set=external_force_set) + + return model + + +def set_dynamics(model, numerical_time_series, dynamics_type_str="torque_driven"): + 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, + with_contact=True, + phase=0, + ) + return dynamics + + +def set_objective_functions(model, dynamics_type): + objective_functions = ObjectiveList() + if isinstance(model, FesMskModel): + objective_functions.add(CustomObjective.minimize_overall_muscle_force_production, custom_type=ObjectiveFcn.Lagrange, weight=1, quadratic=True) + # objective_functions.add(CustomObjective.minimize_overall_muscle_fatigue, custom_type=ObjectiveFcn.Lagrange, weight=1, quadratic=True) + else: + control_key = "tau" if dynamics_type == "torque_driven" else "muscles" + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key=control_key, weight=1000, quadratic=True) + return objective_functions + + +def set_x_init(n_shooting, pedal_config, turn_number): + x_init = InitialGuessList() + + biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod" + q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( + biorbd_model_path, + n_shooting, + x_center=pedal_config["x_center"], + y_center=pedal_config["y_center"], + radius=pedal_config["radius"], + ik_method="lm", + cycling_number=turn_number + ) + x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) + # x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) + # u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) + # u_init.add("tau", u_guess, interpolation=InterpolationType.EACH_FRAME) + + return x_init + + +def set_u_bounds_and_init(bio_model, dynamics_type_str): + u_bounds = BoundsList() + u_init = InitialGuessList() + if dynamics_type_str == "torque_driven": + u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0) + elif dynamics_type_str == "muscle_driven": + muscle_min, muscle_max, muscle_init = 0.0, 1.0, 0.5 + u_bounds.add(key="muscles", + min_bound=np.array([muscle_min] * bio_model.nb_muscles), + max_bound=np.array([muscle_max] * bio_model.nb_muscles), + phase=0) + u_init.add(key="muscles", + initial_guess=np.array([muscle_init] * bio_model.nb_muscles), + phase=0) + return u_bounds, u_init + + +def set_bounds(model, x_init, n_shooting, turn_number, interpolation_type=InterpolationType.CONSTANT, cardinal=4): + x_bounds = BoundsList() + if isinstance(model, FesMskModel): + x_bounds, _ = OcpFesMsk._set_bounds_fes(model) + + q_x_bounds = model.bounds_from_ranges("q") + qdot_x_bounds = model.bounds_from_ranges("qdot") + + if interpolation_type == InterpolationType.EACH_FRAME: + x_min_bound = [] + x_max_bound = [] + for i in range(q_x_bounds.min.shape[0]): + x_min_bound.append([q_x_bounds.min[i][0]] * (n_shooting + 1)) + x_max_bound.append([q_x_bounds.max[i][0]] * (n_shooting + 1)) + + cardinal_node_list = [i * int(n_shooting / ((n_shooting/(n_shooting/turn_number)) * cardinal)) for i in range(int((n_shooting/(n_shooting/turn_number)) * cardinal + 1))] + slack = 10*(np.pi/180) + for i in range(len(x_min_bound[0])): + x_min_bound[0][i] = 0 + x_max_bound[0][i] = 1 + x_min_bound[1][i] = 1 + x_min_bound[2][i] = x_init["q"].init[2][-1] + x_max_bound[2][i] = x_init["q"].init[2][0] + for i in range(len(cardinal_node_list)): + cardinal_index = cardinal_node_list[i] + x_min_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] if i % cardinal == 0 else x_init["q"].init[2][cardinal_index] - slack + x_max_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] if i % cardinal == 0 else x_init["q"].init[2][cardinal_index] + slack + # x_min_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] - slack + # x_max_bound[2][cardinal_index] = x_init["q"].init[2][cardinal_index] + slack + + x_bounds.add(key="q", min_bound=x_min_bound, max_bound=x_max_bound, phase=0, + interpolation=InterpolationType.EACH_FRAME) + + else: + x_bounds.add(key="q", bounds=q_x_bounds, phase=0) + + # Modifying pedal speed bounds + qdot_x_bounds.max[0] = [10, 10, 10] + qdot_x_bounds.min[0] = [-10, -10, -10] + qdot_x_bounds.max[1] = [10, 10, 10] + qdot_x_bounds.min[1] = [-10, -10, -10] + qdot_x_bounds.max[2] = [-2, -2, -2] + qdot_x_bounds.min[2] = [-12, -12, -12] + x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) + return x_bounds + + +def set_constraints(bio_model, n_shooting, turn_number): + constraints = ConstraintList() + superimpose_marker_list = [i * int(n_shooting / ((n_shooting / (n_shooting / turn_number)) * 1)) for i in + range(int((n_shooting / (n_shooting / turn_number)) * 1 + 1))] + + for i in superimpose_marker_list: + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=i, + axes=[Axis.X, Axis.Y], + ) + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=i, + marker_index=bio_model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + ) + + return constraints + + +def main(): + """ + Main function to configure and solve the optimal control problem. + """ + # --- Configuration --- # + dynamics_type = "torque_driven" # Available options: "torque_driven", "muscle_driven", "fes_driven" + model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod" + pulse_width = None + + # NMPC parameters + cycle_duration = 1 + cycle_len = 100 + n_cycles_to_advance = 1 + n_cycles_simultaneous = 3 + n_cycles = 3 + + # Bike parameters + turn_number = n_cycles_simultaneous + pedal_config = {"x_center": 0.35, "y_center": 0.0, "radius": 0.1} + + # --- Load the appropriate model --- # + if dynamics_type in ["torque_driven", "muscle_driven"]: + model = BiorbdModel(model_path) + elif dynamics_type == "fes_driven": + # Define muscle dynamics for the FES-driven model + muscles_model = [ + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusScapula_P", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="TRIlong", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_long", is_approximated=False, sum_stim_truncation=10), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_brevis", is_approximated=False, sum_stim_truncation=10), + ] + stim_time = list(np.linspace(0, cycle_duration*n_cycles_simultaneous, 67)[:-1]) + model = FesMskModel( + name=None, + biorbd_path=model_path, + muscles_model=muscles_model, + stim_time=stim_time, + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=False, + external_force_set=None, # External forces will be added later + ) + pulse_width = {"min": DingModelPulseWidthFrequencyWithFatigue().pd0, "max": 0.0006, "bimapping": False, + "same_for_all_muscles": False, + "fixed": False} + # Adjust n_shooting based on the stimulation time + cycle_len = len(stim_time) + else: + raise ValueError(f"Dynamics type '{dynamics_type}' not recognized") + + nmpc = prepare_nmpc( + model=model, + cycle_duration=cycle_duration, + cycle_len=cycle_len, + n_cycles_to_advance=n_cycles_to_advance, + n_cycles_simultaneous=n_cycles_simultaneous, + total_n_cycles=n_cycles, + turn_number=turn_number, + pedal_config=pedal_config, + pulse_width=pulse_width, + dynamics_type=dynamics_type, + ) + + def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): + return cycle_idx < n_cycles # True if there are still some cycle to perform + + # Add the penalty cost function plot + nmpc.add_plot_penalty(CostType.ALL) + # Solve the optimal control problem + sol = nmpc.solve( + update_functions, + solver=Solver.IPOPT(show_online_optim=False, _max_iter=10000, show_options=dict(show_bounds=True)), + cycle_solutions=MultiCyclicCycleSolutions.ALL_CYCLES, + get_all_iterations=True, + cyclic_options={"states": {}}, + n_cycles_simultaneous=n_cycles_simultaneous, + ) + + # Display graphs and animate the solution + sol[1][0].graphs(show_bounds=True) + sol[1][1].graphs(show_bounds=True) + print(sol[1][1].constraints) + sol[0].graphs(show_bounds=True) + print(sol[0].constraints) + print(sol[0].parameters) + print(sol[0].detailed_cost) + sol[0].animate(viewer="pyorerun", n_frames=200, show_tracked_markers=True) + + +if __name__ == "__main__": + main() diff --git a/cocofest/examples/dynamics/cycling/inverse_kinematics_cycling.py b/cocofest/examples/dynamics/cycling/inverse_kinematics_cycling.py deleted file mode 100644 index 83ae434..0000000 --- a/cocofest/examples/dynamics/cycling/inverse_kinematics_cycling.py +++ /dev/null @@ -1,73 +0,0 @@ -""" -This example will do an inverse kinematics and dynamics of a 100 steps hand cycling motion. -""" - -import numpy as np -import matplotlib.pyplot as plt - -import biorbd -from pyorerun import BiorbdModel, PhaseRerun - -from cocofest import get_circle_coord - - -def main(show_plot=True, animate=True): - # Load a predefined model - model = biorbd.Model("../../msk_models/simplified_UL_Seth.bioMod") - n_frames = 1000 - - # Define the marker target to match - z = model.markers(np.array([0, 0]))[0].to_array()[2] - get_circle_coord_list = np.array( - [get_circle_coord(theta, 0.35, 0, 0.1, z) for theta in np.linspace(0, -2 * np.pi, n_frames)] - ) - target_q = np.array( - [ - [get_circle_coord_list[:, 0]], - [get_circle_coord_list[:, 1]], - [get_circle_coord_list[:, 2]], - ] - ) - - # Perform the inverse kinematics - ik = biorbd.InverseKinematics(model, target_q) - ik_q = ik.solve(method="trf") - ik_qdot = np.array([np.gradient(ik_q[i], (1 / n_frames)) for i in range(ik_q.shape[0])]) - ik_qddot = np.array([np.gradient(ik_qdot[i], (1 / (n_frames))) for i in range(ik_qdot.shape[0])]) - - # Perform the inverse dynamics - tau_shape = (model.nbQ(), ik_q.shape[1] - 1) - tau = np.zeros(tau_shape) - for i in range(tau.shape[1]): - tau_i = model.InverseDynamics(ik_q[:, i], ik_qdot[:, i], ik_qddot[:, i]) - tau[:, i] = tau_i.to_array() - - # Plot the results - if show_plot: - fig, (ax1, ax2) = plt.subplots(1, 2) - ax1.set_title("Q") - ax1.plot(np.linspace(0, 1, n_frames), ik_q[0], color="orange", label="shoulder") - ax1.plot(np.linspace(0, 1, n_frames), ik_q[1], color="blue", label="elbow") - ax1.set(xlabel="Time (s)", ylabel="Angle (rad)") - ax2.set_title("Tau") - ax2.plot(np.linspace(0, 1, n_frames - 1), tau[0], color="orange", label="shoulder") - ax2.plot(np.linspace(0, 1, n_frames - 1), tau[1], color="blue", label="elbow") - ax2.set(xlabel="Time (s)", ylabel="Torque (N.m)") - plt.legend() - plt.show() - - # pyorerun animation - if animate: - biorbd_model = biorbd.Model("../../msk_models/simplified_UL_Seth_full_mesh.bioMod") - prr_model = BiorbdModel.from_biorbd_object(biorbd_model) - - nb_seconds = 1 - t_span = np.linspace(0, nb_seconds, n_frames) - - viz = PhaseRerun(t_span) - viz.add_animated_model(prr_model, ik_q) - viz.rerun("msk_model") - - -if __name__ == "__main__": - main(show_plot=True, animate=True) diff --git a/cocofest/examples/getting_started/model_integration.py b/cocofest/examples/getting_started/model_integration.py index 0c8044d..13d38da 100644 --- a/cocofest/examples/getting_started/model_integration.py +++ b/cocofest/examples/getting_started/model_integration.py @@ -1,24 +1,39 @@ -import matplotlib.pyplot as plt -from cocofest import IvpFes, ModelMaker +""" +This example was build to explain of to integrate a solution and has no objectives nor parameter to optimize. +The uncommented model used is the DingModelFrequencyWithFatigue, but you can change it to any other model. +The model is integrated for 300 seconds and the stimulation will be on for 1 second at 33 Hz and of for a second. +The effect of the fatigue will be visible and the force state result will decrease over time. +""" +from cocofest import (IvpFes, + DingModelFrequencyWithFatigue, + DingModelPulseIntensityFrequencyWithFatigue, + DingModelPulseWidthFrequencyWithFatigue, + FES_plot) +import numpy as np +from bioptim import OdeSolver -# --- Build ocp --- # -# This problem was build to be integrated and has no objectives nor parameter to optimize. -model = ModelMaker.create_model("ding2003_with_fatigue", is_approximated=False) # Can not approximate this model in ivp -fes_parameters = { - "model": model, - "stim_time": [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], -} -ivp_parameters = {"final_time": 1, "use_sx": True} +# --- Set stimulation time apparition --- # +final_time = 300 +stim_time = [val for start in range(0, final_time, 2) for val in np.linspace(start, start + 1, 34)[:-1]] -ivp = IvpFes(fes_parameters, ivp_parameters) +# --- Set Ding2003 model --- # +model = DingModelFrequencyWithFatigue(stim_time=stim_time, sum_stim_truncation=10) +fes_parameters = {"model": model} + +# --- Set Ding2007 model --- # +# model = DingModelPulseWidthFrequencyWithFatigue(stim_time=stim_time, sum_stim_truncation=10) +# fes_parameters = {"model": model, "pulse_width": 0.0003} + +# --- Set Hmed2018 model --- # +# model = DingModelPulseIntensityFrequencyWithFatigue(stim_time=stim_time, sum_stim_truncation=10) +# fes_parameters = {"model": model, "pulse_intensity": 50} + +# --- Build ivp --- # +ivp_parameters = {"final_time": final_time, "ode_solver": OdeSolver.RK1(n_integration_steps=10)} +ivp = IvpFes(fes_parameters=fes_parameters, ivp_parameters=ivp_parameters) result, time = ivp.integrate() +result["time"] = time # Plotting the force state result -plt.title("Force state result") - -plt.plot(time, result["F"][0], color="blue", label="force") -plt.xlabel("time (s)") -plt.ylabel("force (N)") -plt.legend() -plt.show() +FES_plot(data=result).plot(title="FES model integration") diff --git a/cocofest/examples/getting_started/pulse_duration_optimization.py b/cocofest/examples/getting_started/pulse_duration_optimization.py index 293c8bc..964d5ec 100644 --- a/cocofest/examples/getting_started/pulse_duration_optimization.py +++ b/cocofest/examples/getting_started/pulse_duration_optimization.py @@ -2,34 +2,39 @@ This example will do a 10 stimulation example with Ding's 2007 pulse width and frequency model. This ocp was build to match a force value of 200N at the end of the last node. """ - -from bioptim import Solver -from cocofest import OcpFes, ModelMaker +import numpy as np +from bioptim import Solver, OdeSolver +from cocofest import OcpFes, ModelMaker, ModifiedOdeSolverRK4, FES_plot # --- Build ocp --- # # This ocp was build to match a force value of 200N at the end of the last node. -# The stimulation will be optimized between 0.01 to 0.1 seconds and are equally spaced (a fixed frequency). -# Plus the pulsation width will be optimized between 0 and 0.0006 seconds and are not the same across the problem. -# The flag with_fatigue is set to True by default, this will include the fatigue model +# The stimulation will optimized the pulsation width between 0 and 0.0006 seconds and the pulse width will be +# similar across the problem "bimapping": True,". -model = ModelMaker.create_model("ding2007_with_fatigue", is_approximated=False) +final_time = 1 +model = ModelMaker.create_model("ding2007_with_fatigue", is_approximated=False, sum_stim_truncation=10, + stim_time=list(np.linspace(0, final_time, 34)[:-1]), + previous_stim={"time": [-0.15, -0.10, -0.05], + "pulse_width": [0.0005, 0.0005, 0.0005]}) minimum_pulse_width = model.pd0 ocp = OcpFes().prepare_ocp( model=model, - stim_time=[0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45], - final_time=0.5, + final_time=final_time, pulse_width={ "min": minimum_pulse_width, "max": 0.0006, "bimapping": True, }, - objective={"end_node_tracking": 100}, + objective={"end_node_tracking": 200}, use_sx=True, n_threads=5, + ode_solver=ModifiedOdeSolverRK4(n_integration_steps=10), + # ode_solver=OdeSolver.RK1(n_integration_steps=10), ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(_hessian_approximation="limited-memory")) + # --- Show results --- # -sol.graphs() +FES_plot(data=sol).plot(title="Optimize pulse width", show_bounds=False, show_stim=False) diff --git a/cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces.py b/cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces.py new file mode 100644 index 0000000..40a84de --- /dev/null +++ b/cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces.py @@ -0,0 +1,43 @@ +""" +This example will do a 10 stimulation example with Ding's 2007 frequency model. +This ocp was build to produce a elbow motion from 5 to 120 degrees. +The stimulation frequency will be optimized between 10 and 100 Hz and pulse duration between minimal sensitivity +threshold and 600us to satisfy the flexion and minimizing required elbow torque control. +External forces will be applied to the system to simulate a real-world scenario. +""" +import numpy as np +from bioptim import Solver + +from cocofest import DingModelPulseWidthFrequencyWithFatigue, OcpFesMsk, FesMskModel + +model = FesMskModel( + name=None, + biorbd_path="../msk_models/arm26_biceps_1dof.bioMod", + muscles_model=[DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIClong")], + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=True, + external_force_set=None, # External forces will be added later +) + +minimum_pulse_duration = DingModelPulseWidthFrequencyWithFatigue().pd0 +ocp = OcpFesMsk.prepare_ocp( + model=model, + stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], + final_time=1, + pulse_width={ + "min": minimum_pulse_duration, + "max": 0.0006, + "bimapping": False, + }, + objective={"minimize_residual_torque": True}, + msk_info={ + "bound_type": "start_end", + "bound_data": [[5], [120]], + "with_residual_torque": True}, + external_forces={"Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, -1]), "with_contact": False}, +) + +sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=2000)) +sol.animate() +sol.graphs(show_bounds=False) diff --git a/cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces_cycling.py b/cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces_cycling.py new file mode 100644 index 0000000..acbe888 --- /dev/null +++ b/cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces_cycling.py @@ -0,0 +1,56 @@ +""" +This example will do a 10 stimulation example with Ding's 2007 frequency model. +This ocp was build to produce a hand cycling motion. +The pulse duration will be optimized between minimal sensitivity threshold and 600us to satisfy the motion while +minimizing residual joints torques and produced muscular forces. +External forces will be applied to the system to simulate a real-world scenario with contacts at the pedal center. +""" +import numpy as np +from bioptim import Solver, ControlType +from cocofest import DingModelPulseWidthFrequencyWithFatigue, OcpFesMsk, FesMskModel, SolutionToPickle, PickleAnimate +import biorbd + +model = FesMskModel( + name=None, + biorbd_path="../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod", + muscles_model=[ + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A", is_approximated=False), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusScapula_P", is_approximated=False), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="TRIlong", is_approximated=False), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_long", is_approximated=False), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_brevis", is_approximated=False), + ], + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=True, + external_force_set=None, # External forces will be added +) + +minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 +ocp = OcpFesMsk.prepare_ocp_for_cycling( + model=model, + stim_time=list(np.round(np.linspace(0, 1, 11), 3))[:-1], + final_time=1, + pulse_width={ + "min": minimum_pulse_width, + "max": 0.0006, + "bimapping": False, + }, + msk_info={"with_residual_torque": True}, + objective={ + "cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1, "target": "marker"}, + "minimize_residual_torque": True, + "minimize_muscle_force": True, + }, + initial_guess_warm_start=False, + external_forces={"Segment_application": "wheel", "torque": np.array([0, 0, 0]), "with_contact": True}, + control_type=ControlType.CONSTANT, +) + +sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) +SolutionToPickle(sol, "hand_cycling_external_forces1.pkl", "").pickle() +biorbd_model = biorbd.Model("../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod") +PickleAnimate("hand_cycling_external_forces1.pkl").animate(model=biorbd_model) + +sol.animate(show_tracked_markers=True) +sol.graphs(show_bounds=True) diff --git a/cocofest/examples/getting_started/pulse_duration_optimization_musculoskeletal_dynamic.py b/cocofest/examples/getting_started/pulse_duration_optimization_musculoskeletal_dynamic.py index 56034ba..74701de 100644 --- a/cocofest/examples/getting_started/pulse_duration_optimization_musculoskeletal_dynamic.py +++ b/cocofest/examples/getting_started/pulse_duration_optimization_musculoskeletal_dynamic.py @@ -12,6 +12,7 @@ name=None, biorbd_path="../msk_models/arm26_biceps_1dof.bioMod", muscles_model=[DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIClong")], + stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], activate_force_length_relationship=True, activate_force_velocity_relationship=True, activate_residual_torque=True, @@ -20,7 +21,6 @@ minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 ocp = OcpFesMsk.prepare_ocp( model=model, - stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], final_time=1, pulse_width={ "min": minimum_pulse_width, diff --git a/cocofest/examples/getting_started/pulse_duration_optimization_nmpc_cyclic.py b/cocofest/examples/getting_started/pulse_duration_optimization_nmpc_cyclic.py index c492e30..e993f8a 100644 --- a/cocofest/examples/getting_started/pulse_duration_optimization_nmpc_cyclic.py +++ b/cocofest/examples/getting_started/pulse_duration_optimization_nmpc_cyclic.py @@ -1,100 +1,248 @@ """ -This example showcases a moving time horizon simulation problem of cyclic muscle force tracking. +This example showcases a moving time horizon simulation problem of cyclic muscle force tracking at last node. The FES model used here is Ding's 2007 pulse width and frequency model with fatigue. -Only the pulse width is optimized, frequency is fixed. +Only the pulse width is optimized to minimize muscle force production, frequency is fixed at 33 Hz. The nmpc cyclic problem stops once the last cycle is reached. """ -import matplotlib.pyplot as plt import numpy as np -from bioptim import Solver, SolutionMerge -from cocofest import NmpcFes, DingModelPulseWidthFrequencyWithFatigue - - -# --- Building force to track ---# -target_time = np.linspace(0, 1, 100) -target_force = abs(np.sin(target_time * np.pi)) * 200 -force_tracking = [target_time, target_force] - -# --- Build nmpc cyclic --- # -cycles_len = 1000 -cycle_duration = 1 -n_cycles = 8 - -minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 -fes_model = DingModelPulseWidthFrequencyWithFatigue(sum_stim_truncation=10) -fes_model.alpha_a = -4.0 * 10e-1 # Increasing the fatigue rate to make the fatigue more visible - -nmpc = NmpcFes.prepare_nmpc( - model=fes_model, - stim_time=list(np.round(np.linspace(0, 1, 31), 3))[:-1], - cycle_len=cycles_len, - cycle_duration=cycle_duration, - pulse_width={ - "min": minimum_pulse_width, - "max": 0.0006, - "bimapping": False, - }, - objective={"force_tracking": force_tracking}, - use_sx=True, - n_threads=5, -) -n_cycles_total = 8 +from bioptim import ( + ConstraintList, + CostType, + ConstraintFcn, + InitialGuessList, + InterpolationType, + MultiCyclicCycleSolutions, + MultiCyclicNonlinearModelPredictiveControl, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + SolutionMerge, + VariableScaling, + Solution, + Solver, + ParameterList, + OptimalControlProgram +) +from casadi import SX -def update_functions(_nmpc, cycle_idx, _sol): - return cycle_idx < n_cycles_total # True if there are still some cycle to perform +from cocofest import ( + DingModelPulseWidthFrequencyWithFatigue, + OcpFes, + FesModel, + FES_plot, + ModifiedOdeSolverRK4 +) -sol = nmpc.solve( - update_functions, - solver=Solver.IPOPT(), - cyclic_options={"states": {}}, - get_all_iterations=True, -) -sol_merged = sol[0].decision_states(to_merge=[SolutionMerge.PHASES, SolutionMerge.NODES]) - -time = sol[0].decision_time(to_merge=SolutionMerge.KEYS, continuous=True) -time = [float(j) for j in time] -fatigue = sol_merged["A"][0] -force = sol_merged["F"][0] - -ax1 = plt.subplot(221) -ax1.plot(time, fatigue, label="A", color="green") -ax1.set_title("Fatigue", weight="bold") -ax1.set_xlabel("Time (s)") -ax1.set_ylabel("Force scaling factor (-)") -plt.legend() - -ax2 = plt.subplot(222) -ax2.plot(time, force, label="F", color="red", linewidth=4) -for i in range(n_cycles): - if i == 0: - ax2.plot(target_time, target_force, label="Target", color="purple") - else: - ax2.plot(target_time + i, target_force, color="purple") -ax2.set_title("Force", weight="bold") -ax2.set_xlabel("Time (s)") -ax2.set_ylabel("Force (N)") -plt.legend() - -barWidth = 0.25 # set width of bar -cycles = [sol[1][i].parameters["pulse_width"] for i in range(len(sol[1]))] # set height of bar -bar = [] # Set position of bar on X axis -for i in range(n_cycles): - if i == 0: - br = [barWidth * (x + 1) for x in range(len(cycles[i]))] - else: - br = [bar[-1][-1] + barWidth * (x + 1) for x in range(len(cycles[i]))] - bar.append(br) - -ax3 = plt.subplot(212) -for i in range(n_cycles): - ax3.bar(bar[i], cycles[i], width=barWidth, edgecolor="grey", label=f"cycle n°{i+1}") -ax3.set_xticks([np.mean(r) for r in bar], [str(i + 1) for i in range(n_cycles)]) -ax3.set_xlabel("Cycles") -ax3.set_ylabel("Pulse width (s)") -plt.legend() -ax3.set_title("Pulse width", weight="bold") -plt.show() +class MyCyclicNMPC(MultiCyclicNonlinearModelPredictiveControl): + def __init__(self, **kwargs): + super(MyCyclicNMPC, self).__init__(**kwargs) + self.all_models = [] + self.cycle_duration = kwargs["cycle_duration"] + self.bimapped_param = True if self.parameters.shape == 1 else False + + def advance_window_bounds_states(self, sol, n_cycles_simultaneous=None, **extra): + super(MyCyclicNMPC, self).advance_window_bounds_states(sol) + self.update_stim(sol) + return True + + def advance_window_initial_guess_states(self, sol, n_cycles_simultaneous=None): + super(MyCyclicNMPC, self).advance_window_initial_guess_states(sol) + # self.ocp_solver.ocp.parameter_init["pulse_width"].init[:, :] = sol.parameters["pulse_width"][:, None] + # TODO : Add parameters initial guess + return True + + def update_stim(self, sol): + truncation_term = self.nlp[0].model._sum_stim_truncation + solution_stimulation_time = self.nlp[0].model.stim_time[-truncation_term:] + previous_stim_time = [x - self.phase_time[0] for x in solution_stimulation_time] + stimulation_per_cycle = int(len(self.nlp[0].model.stim_time) / self.n_cycles) + previous_pw_time = list(sol.parameters["pulse_width"]) * len(previous_stim_time) if self.bimapped_param else list(sol.parameters["pulse_width"][:stimulation_per_cycle][-truncation_term:]) + previous_stim = {"time": previous_stim_time, "pulse_width": previous_pw_time} + new_model = DingModelPulseWidthFrequencyWithFatigue( + previous_stim=previous_stim, + stim_time=self.nlp[0].model.stim_time, + sum_stim_truncation=truncation_term + ) + self.nlp[0].model = new_model + self.all_models.append(new_model) + + def _initialize_solution(self, dt: float, states: list, controls: list, parameters: list): + combined_model = self.create_model_from_list(self.all_models) + x_init = InitialGuessList() + for key in self.nlp[0].states.keys(): + x_init.add( + key, + np.concatenate([state[key][:, :-1] for state in states] + [states[-1][key][:, -1:]], axis=1), + interpolation=InterpolationType.EACH_FRAME, + phase=0, + ) + + u_init = InitialGuessList() + for key in self.nlp[0].controls.keys(): + controls_tp = np.concatenate([control[key] for control in controls], axis=1) + u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME, phase=0) + + p_init = InitialGuessList() + stimulation_per_cycle = int(len(self.nlp[0].model.stim_time) / self.n_cycles) + for key in self.nlp[0].parameters.keys(): + combined_parameters = [list(parameters[i][key]) * stimulation_per_cycle for i in range(len(parameters))] if self.bimapped_param else [list(parameter[key][:stimulation_per_cycle]) for parameter in parameters] + combined_parameters = [val for sublist in combined_parameters for val in sublist] + p_init[key] = combined_parameters + + parameters = ParameterList(use_sx=self.cx == SX) + for key in self.nlp[0].parameters.keys(): + parameters.add( + name=key, + function=self.nlp[0].parameters[key].function, + size=len(combined_parameters), + scaling=VariableScaling(key, [1] * len(combined_parameters)), + ) + + solution_ocp = OptimalControlProgram( + bio_model=[combined_model], + dynamics=self.nlp[0].dynamics_type, + n_shooting=self.total_optimization_run * self.cycle_len, + phase_time=self.total_optimization_run * self.cycle_len * dt, + x_init=x_init, + u_init=u_init, + use_sx=self.cx == SX, + parameters=parameters, + parameter_init=p_init, + # parameter_bounds=self.parameter_bounds, + ode_solver=self.nlp[0].ode_solver, + ) + a_init = InitialGuessList() + return Solution.from_initial_guess(solution_ocp, [np.array([dt]), x_init, u_init, p_init, a_init]) + + def create_model_from_list(self, models: list): + stimulation_per_cycle = int(len(self.nlp[0].model.stim_time) / self.n_cycles) + stim_time = [] + for i in range(len(models)): + stim_time.append(list(np.array(models[0].stim_time[:stimulation_per_cycle]) + (i * self.cycle_duration))) + stim_time = [val for sublist in stim_time for val in sublist] + + combined_model = DingModelPulseWidthFrequencyWithFatigue( + stim_time=stim_time, + sum_stim_truncation=self.nlp[0].model._sum_stim_truncation + ) + return combined_model + + +def prepare_nmpc( + model: FesModel, + cycle_duration: int | float, + n_cycles_to_advance: int, + n_cycles_simultaneous: int, + pulse_width: dict, + use_sx: bool = False, +): + + dynamics = OcpFes._declare_dynamics(model) + cycle_len = OcpFes.prepare_n_shooting(model.stim_time, cycle_duration * n_cycles_simultaneous) + cycle_len = cycle_len / n_cycles_simultaneous + + # Define objective functions + objective_functions = ObjectiveList() + objective_functions.add( + ObjectiveFcn.Lagrange.MINIMIZE_STATE, + key="F", + weight=1, + quadratic=True) + + x_bounds, x_init = OcpFes._set_bounds(model) + + (parameters, parameters_bounds, parameters_init, parameter_objectives) = OcpFes._build_parameters( + model=model, + pulse_width=pulse_width, + pulse_intensity=None, + use_sx=use_sx, + ) + OcpFes.update_model_param(model, parameters) + + constraints_node_list = [33 * (1 + 2 * i) for i in range(int(cycle_len * n_cycles_simultaneous) // (2 * 33))] + constraints = ConstraintList() + for i in constraints_node_list: + constraints.add(ConstraintFcn.TRACK_STATE, key="F", node=i, min_bound=150, max_bound=160) + + return MyCyclicNMPC( + bio_model=model, + dynamics=dynamics, + cycle_len=int(cycle_len), + cycle_duration=cycle_duration, + n_cycles_simultaneous=n_cycles_simultaneous, + n_cycles_to_advance=n_cycles_to_advance, + common_objective_functions=objective_functions, + constraints=constraints, + x_bounds=x_bounds, + x_init=x_init, + parameters=parameters, + parameter_bounds=parameters_bounds, + parameter_init=parameters_init, + parameter_objectives=parameter_objectives, + ode_solver=OdeSolver.RK2(n_integration_steps=10), + # ode_solver=ModifiedOdeSolverRK4(n_integration_steps=10), + n_threads=20, + use_sx=use_sx, + ) + + +def main(): + """ + Main function to configure and solve the optimal control problem. + """ + # --- Build nmpc cyclic --- # + cycle_duration = 2 + n_cycles_simultaneous = 3 + n_cycles_to_advance = 1 + n_cycles = 5 + + # --- Set stimulation time apparition --- # + final_time = 2 * n_cycles_simultaneous + stim_time = [val for start in range(0, final_time, 2) for val in np.linspace(start, start + 1, 34)[:-1]] + + # --- Build FES model --- # + minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 + fes_model = DingModelPulseWidthFrequencyWithFatigue(stim_time=stim_time, sum_stim_truncation=10) + + nmpc = prepare_nmpc( + model=fes_model, + cycle_duration=cycle_duration, + n_cycles_to_advance=n_cycles_to_advance, + n_cycles_simultaneous=n_cycles_simultaneous, + pulse_width={"min": minimum_pulse_width, + "max": 0.0006, + "bimapping": True, + "fixed": False}, + use_sx=False, + ) + + def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): + return cycle_idx < n_cycles # True if there are still some cycle to perform + + # Add the penalty cost function plot + nmpc.add_plot_penalty(CostType.ALL) + # Solve the optimal control problem + sol = nmpc.solve( + update_functions, + solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), + cycle_solutions=MultiCyclicCycleSolutions.ALL_CYCLES, + get_all_iterations=True, + cyclic_options={"states": {}}, + n_cycles_simultaneous=n_cycles_simultaneous, + ) + + result = sol[0].stepwise_states(to_merge=[SolutionMerge.NODES]) + time = sol[0].stepwise_time(to_merge=[SolutionMerge.NODES]).T[0] + result["time"] = time + + # Plotting the force state result + FES_plot(data=result).plot(title="NMPC FES model optimization") + print(sol[0].parameters) + + +if __name__ == "__main__": + main() diff --git a/cocofest/examples/getting_started/pulse_intensity_optimization.py b/cocofest/examples/getting_started/pulse_intensity_optimization.py index b8cc12e..6c5c11c 100644 --- a/cocofest/examples/getting_started/pulse_intensity_optimization.py +++ b/cocofest/examples/getting_started/pulse_intensity_optimization.py @@ -13,8 +13,7 @@ DingModelPulseIntensityFrequencyWithFatigue() ) ocp = OcpFes().prepare_ocp( - model=DingModelPulseIntensityFrequencyWithFatigue(is_approximated=False), - stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], + model=DingModelPulseIntensityFrequencyWithFatigue(is_approximated=False, stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9]), final_time=1, pulse_intensity={ "min": minimum_pulse_intensity, diff --git a/cocofest/examples/msk_models/arm26_cycling_pedal.bioMod b/cocofest/examples/msk_models/arm26_cycling_pedal.bioMod new file mode 100644 index 0000000..dfd5d7b --- /dev/null +++ b/cocofest/examples/msk_models/arm26_cycling_pedal.bioMod @@ -0,0 +1,417 @@ +version 3 +gravity 0 -9.81 0 + +// SEGMENT DEFINITION +segment base + // meshfile mesh/ground_ribs.vtp +endsegment + +segment r_humerus_translation + parent base + RTinMatrix 1 + RT + 1.0 0.0 0.0 -0.017545 + 0.0 1.0 0.0 -0.007 + 0.0 0.0 1.0 0.17 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_humerus_rotation1 + parent r_humerus_translation + RTinMatrix 1 + RT + 0.9975010776109747 0.039020807762349584 -0.058898019716436364 0.0 + -0.038952964437603196 0.9992383982621832 0.0022999999889266845 0.0 + 0.05894291073968768 0.0 0.9982613551938856 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ -2*pi 2*pi +endsegment + + +segment r_humerus_rotation2 + parent r_humerus_rotation1 +endsegment + +// Segment +segment r_humerus_rotation3 + parent r_humerus_rotation2 + RTinMatrix 1 + RT + 0.0 -0.0588981755023151 0.9982639956056206 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.9982639956056206 0.0588981755023151 0.0 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_humerus + parent r_humerus_rotation3 + RTinMatrix 1 + RT + 0.039020807762349605 0.9992383982621836 0.0 0.0 + -0.11754676602826802 0.004590265714620227 0.9930567391931666 0.0 + 0.9923004254548464 -0.03874987611716229 0.11763635808301447 0.0 + 0.0 0.0 0.0 1.0 + mass 1.8645719999999999 + inertia + 0.01481 0.0 0.0 + 0.0 0.004551 0.0 + 0.0 0.0 0.013193 + com 0 -0.18049599999999999 0 + meshfile mesh/arm_r_humerus.vtp +endsegment + + +segment r_ulna_radius_hand_translation + parent r_humerus + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.0061 + 0.0 1.0 0.0 -0.2904 + 0.0 0.0 1.0 -0.0123 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_ulna_radius_hand_rotation1 + parent r_ulna_radius_hand_translation + RTinMatrix 1 + RT + 0.801979522152563 -0.5953053712684071 0.04940000998917986 0.0 + 0.5941792022021661 0.8034995425879125 0.036600009991983457 0.0 + -0.06148106796684942 3.469446951953614e-18 0.9981082497813831 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ 0 pi +endsegment + +segment r_ulna_radius_hand_rotation2 + parent r_ulna_radius_hand_rotation1 +endsegment + + +segment r_ulna_radius_hand_rotation3 + parent r_ulna_radius_hand_rotation2 + RTinMatrix 1 + RT + 0.0 0.049433130424779516 0.998777435476196 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.998777435476196 -0.049433130424779516 0.0 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_ulna_radius_hand + parent r_ulna_radius_hand_rotation3 + RTinMatrix 1 + RT + -0.5953053712684069 0.803499542587912 0.0 0.0 + 0.08898397360606149 0.06592740211634747 0.9938487963928239 0.0 + 0.7985570533031812 0.5916435267212894 -0.11074551868375905 0.0 + 0.0 0.0 0.0 1.0 + mass 1.5343150000000001 + inertia + 0.019281 0.0 0.0 + 0.0 0.001571 0.0 + 0.0 0.0 0.020062 + com 0 -0.181479 0 + meshfile mesh/arm_r_ulna.vtp +endsegment + + marker COM_hand + parent r_ulna_radius_hand + position 0 -0.181479 0 + endmarker + +//Pedal segment +variables + $wheel_radius 0.1 + $wheel_x_position 0.3 // X position of the wheel + $wheel_y_position 0.0 // Y position of the wheel +endvariables + +segment wheel_rotation + parent r_ulna_radius_hand + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 -0.181479 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ -2*pi 2*pi +endsegment + +segment wheel + parent wheel_rotation + RT 0 0 0 xyz $wheel_radius 0 0 + mass 1 + inertia + 1 0 0 + 0 1 0 + 0 0 1 + mesh 0 0 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0.93969262*$wheel_radius 0 0.2 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 +endsegment + + // Markers + marker wheel_center + parent wheel + position 0 0 0 + endmarker + + marker global_wheel_center + parent base + position $wheel_x_position $wheel_y_position 0 + endmarker + + +// MUSCLE DEFINITION + +musclegroup base_to_r_ulna_radius_hand + OriginParent base + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlong + Type degrootefatigable + musclegroup base_to_r_ulna_radius_hand + OriginPosition -0.053650000000000003 -0.013729999999999999 0.14723 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.13400000000000001 + maximalForce 798.51999999999998 + tendonSlackLength 0.14299999999999999 + pennationAngle 0.20943951 + maxVelocity 10 + + fatigueParameters + Type Xia + fatiguerate 0.01 + recoveryrate 0.002 + developfactor 10 + recoveryfactor 10 + endfatigueparameters + endmuscle + + viapoint TRIlong-P2 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.027140000000000001 -0.11441 -0.0066400000000000001 + endviapoint + viapoint TRIlong-P3 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRIlong-P4 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle BIClong + Type degroote + musclegroup base_to_r_ulna_radius_hand + OriginPosition -0.039234999999999999 0.00347 0.14795 + InsertionPosition 0.0075100000000000002 -0.048390000000000002 0.02179 + optimalLength 0.1157 + maximalForce 624.29999999999995 + tendonSlackLength 0.27229999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BIClong-P2 + parent base + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position -0.028944999999999999 0.01391 0.15639 + endviapoint + viapoint BIClong-P3 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.021309999999999999 0.017930000000000001 0.010279999999999999 + endviapoint + viapoint BIClong-P4 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.023779999999999999 -0.00511 0.01201 + endviapoint + viapoint BIClong-P5 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01345 -0.02827 0.0013600000000000001 + endviapoint + viapoint BIClong-P6 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01068 -0.077359999999999998 -0.00165 + endviapoint + viapoint BIClong-P7 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01703 -0.12125 0.00024000000000000001 + endviapoint + viapoint BIClong-P8 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.022800000000000001 -0.1754 -0.0063 + endviapoint + + muscle BICshort + Type degroote + musclegroup base_to_r_ulna_radius_hand + OriginPosition 0.0046750000000000003 -0.01231 0.13475000000000001 + InsertionPosition 0.0075100000000000002 -0.048390000000000002 0.02179 + optimalLength 0.1321 + maximalForce 435.56 + tendonSlackLength 0.1923 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BICshort-P2 + parent base + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position -0.0070749999999999997 -0.040039999999999999 0.14507 + endviapoint + viapoint BICshort-P3 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.011169999999999999 -0.075759999999999994 -0.011010000000000001 + endviapoint + viapoint BICshort-P4 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.01703 -0.12125 -0.010789999999999999 + endviapoint + viapoint BICshort-P5 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.022800000000000001 -0.1754 -0.0063 + endviapoint + +// r_humerus > r_ulna_radius_hand +musclegroup r_humerus_to_r_ulna_radius_hand + OriginParent r_humerus + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlat + Type degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition -0.0059899999999999997 -0.12645999999999999 0.00428 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.1138 + maximalForce 624.29999999999995 + tendonSlackLength 0.098000000000000004 + pennationAngle 0.15707963 + maxVelocity 10 + endmuscle + + viapoint TRIlat-P2 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.023439999999999999 -0.14527999999999999 0.0092800000000000001 + endviapoint + viapoint TRIlat-P3 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRIlat-P4 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle TRImed + Type degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition -0.0083800000000000003 -0.13694999999999999 -0.0090600000000000003 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.1138 + maximalForce 624.29999999999995 + tendonSlackLength 0.090800000000000006 + pennationAngle 0.15707963 + maxVelocity 10 + endmuscle + + viapoint TRImed-P2 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.026009999999999998 -0.15139 -0.010800000000000001 + endviapoint + viapoint TRImed-P3 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRImed-P4 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle BRA + Type degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition 0.0067999999999999996 -0.1739 -0.0035999999999999999 + InsertionPosition -0.0032000000000000002 -0.023900000000000001 0.00089999999999999998 + optimalLength 0.085800000000000001 + maximalForce 987.25999999999999 + tendonSlackLength 0.053499999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle diff --git a/cocofest/examples/msk_models/arm26_cycling_pedal_aligned.bioMod b/cocofest/examples/msk_models/arm26_cycling_pedal_aligned.bioMod new file mode 100644 index 0000000..7b4c85c --- /dev/null +++ b/cocofest/examples/msk_models/arm26_cycling_pedal_aligned.bioMod @@ -0,0 +1,412 @@ +version 3 +gravity 0 -9.81 0 + +// SEGMENT DEFINITION +segment base + // meshfile mesh/ground_ribs.vtp +endsegment + +segment r_humerus_translation + parent base + RTinMatrix 1 + RT + 1.0 0.0 0.0 -0.017545 + 0.0 1.0 0.0 -0.007 + 0.0 0.0 1.0 0.17 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_humerus_rotation1 + parent r_humerus_translation + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ -2*pi 2*pi +endsegment + + +segment r_humerus_rotation2 + parent r_humerus_rotation1 +endsegment + +// Segment +segment r_humerus_rotation3 + parent r_humerus_rotation2 + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_humerus + parent r_humerus_rotation3 + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + mass 1.8645719999999999 + inertia + 0.01481 0.0 0.0 + 0.0 0.004551 0.0 + 0.0 0.0 0.013193 + com 0 -0.18049599999999999 0 + meshfile mesh/arm_r_humerus.vtp +endsegment + + +segment r_ulna_radius_hand_translation + parent r_humerus + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.0061 + 0.0 1.0 0.0 -0.2904 + 0.0 0.0 1.0 -0.0123 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_ulna_radius_hand_rotation1 + parent r_ulna_radius_hand_translation + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ 0 pi +endsegment + +segment r_ulna_radius_hand_rotation2 + parent r_ulna_radius_hand_rotation1 +endsegment + + +segment r_ulna_radius_hand_rotation3 + parent r_ulna_radius_hand_rotation2 + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 +endsegment + + +segment r_ulna_radius_hand + parent r_ulna_radius_hand_rotation3 + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + mass 1.5343150000000001 + inertia + 0.019281 0.0 0.0 + 0.0 0.001571 0.0 + 0.0 0.0 0.020062 + com 0 -0.181479 0 + meshfile mesh/arm_r_ulna.vtp +endsegment + + marker COM_hand + parent r_ulna_radius_hand + position 0 -0.181479 0 + endmarker + +//Pedal segment +variables + $wheel_radius 0.1 + $wheel_x_position 0.3 // X position of the wheel + $wheel_y_position 0.0 // Y position of the wheel +endvariables + +segment wheel_rotation + parent r_ulna_radius_hand + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 -0.181479 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ -2*pi 2*pi +endsegment + +segment wheel + parent wheel_rotation + RT 0 0 0 xyz $wheel_radius 0 0 + mass 1 + inertia + 1 0 0 + 0 1 0 + 0 0 1 + mesh 0 0 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 +endsegment + + // Markers + marker wheel_center + parent wheel + position 0 0 0 + endmarker + + marker global_wheel_center + parent base + position $wheel_x_position $wheel_y_position 0 + endmarker + + +// MUSCLE DEFINITION + +musclegroup base_to_r_ulna_radius_hand + OriginParent base + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlong + Type degrootefatigable + musclegroup base_to_r_ulna_radius_hand + OriginPosition -0.053650000000000003 -0.013729999999999999 0.14723 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.13400000000000001 + maximalForce 798.51999999999998 + tendonSlackLength 0.14299999999999999 + pennationAngle 0.20943951 + maxVelocity 10 + + fatigueParameters + Type Xia + fatiguerate 0.01 + recoveryrate 0.002 + developfactor 10 + recoveryfactor 10 + endfatigueparameters + endmuscle + + viapoint TRIlong-P2 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.027140000000000001 -0.11441 -0.0066400000000000001 + endviapoint + viapoint TRIlong-P3 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRIlong-P4 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle BIClong + Type degroote + musclegroup base_to_r_ulna_radius_hand + OriginPosition -0.039234999999999999 0.00347 0.14795 + InsertionPosition 0.0075100000000000002 -0.048390000000000002 0.02179 + optimalLength 0.1157 + maximalForce 624.29999999999995 + tendonSlackLength 0.27229999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BIClong-P2 + parent base + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position -0.028944999999999999 0.01391 0.15639 + endviapoint + viapoint BIClong-P3 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.021309999999999999 0.017930000000000001 0.010279999999999999 + endviapoint + viapoint BIClong-P4 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.023779999999999999 -0.00511 0.01201 + endviapoint + viapoint BIClong-P5 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01345 -0.02827 0.0013600000000000001 + endviapoint + viapoint BIClong-P6 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01068 -0.077359999999999998 -0.00165 + endviapoint + viapoint BIClong-P7 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01703 -0.12125 0.00024000000000000001 + endviapoint + viapoint BIClong-P8 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.022800000000000001 -0.1754 -0.0063 + endviapoint + + muscle BICshort + Type degroote + musclegroup base_to_r_ulna_radius_hand + OriginPosition 0.0046750000000000003 -0.01231 0.13475000000000001 + InsertionPosition 0.0075100000000000002 -0.048390000000000002 0.02179 + optimalLength 0.1321 + maximalForce 435.56 + tendonSlackLength 0.1923 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BICshort-P2 + parent base + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position -0.0070749999999999997 -0.040039999999999999 0.14507 + endviapoint + viapoint BICshort-P3 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.011169999999999999 -0.075759999999999994 -0.011010000000000001 + endviapoint + viapoint BICshort-P4 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.01703 -0.12125 -0.010789999999999999 + endviapoint + viapoint BICshort-P5 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.022800000000000001 -0.1754 -0.0063 + endviapoint + +// r_humerus > r_ulna_radius_hand +musclegroup r_humerus_to_r_ulna_radius_hand + OriginParent r_humerus + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlat + Type degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition -0.0059899999999999997 -0.12645999999999999 0.00428 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.1138 + maximalForce 624.29999999999995 + tendonSlackLength 0.098000000000000004 + pennationAngle 0.15707963 + maxVelocity 10 + endmuscle + + viapoint TRIlat-P2 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.023439999999999999 -0.14527999999999999 0.0092800000000000001 + endviapoint + viapoint TRIlat-P3 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRIlat-P4 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle TRImed + Type degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition -0.0083800000000000003 -0.13694999999999999 -0.0090600000000000003 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.1138 + maximalForce 624.29999999999995 + tendonSlackLength 0.090800000000000006 + pennationAngle 0.15707963 + maxVelocity 10 + endmuscle + + viapoint TRImed-P2 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.026009999999999998 -0.15139 -0.010800000000000001 + endviapoint + viapoint TRImed-P3 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRImed-P4 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle BRA + Type degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition 0.0067999999999999996 -0.1739 -0.0035999999999999999 + InsertionPosition -0.0032000000000000002 -0.023900000000000001 0.00089999999999999998 + optimalLength 0.085800000000000001 + maximalForce 987.25999999999999 + tendonSlackLength 0.053499999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle diff --git a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod new file mode 100644 index 0000000..2ce821a --- /dev/null +++ b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod @@ -0,0 +1,394 @@ +version 4 + +// File extracted from /simplified_UL_Seth.osim + +//Publication : Holzbaur, K.R.S., Murray, W.M., Delp, S.L. A Model of the Upper Extremity for Simulating Musculoskeletal Surgery and Analyzing Neuromuscular Control. Annals of Biomedical Engineering, vol 33, pp 829�840, 2005 + +//Credit : The OpenSim Development Team (Reinbolt, J; Seth, A; Habib, A; Hamner, S) adapted from a model originally created by Kate Holzbaur (11/22/04) License: Creative Commons (CCBY 3.0). You are free to distribute, remix, tweak, and build upon this work, even commercially, as long as you credit us for the original creation. http://creativecommons.org/licenses/by/3.0/ + +//Force units : N + +//Length units : meters + + +gravity 0 -9.8065999999999995 0 + +// SEGMENT DEFINITION + +// Information about ground segment + segment base + // meshfile mesh/ground_ribs.vtp + endsegment + +// Information about r_humerus segment + // Segment + segment r_humerus_parent_offset + parent ground + RTinMatrix 0 + RT 0 0 0 xyz -0.017545000000000002 -0.0070000000000000001 0.17000000000000001 + endsegment + + // Segments to define transformation axis. + // Segment + segment r_humerus_translation + parent r_humerus_parent_offset + RTinMatrix 1 + RT + 1.0 0.0 0.0 0 + 0.0 1.0 0.0 0 + 0.0 0.0 1.0 0 + 0 0 0 1 + endsegment + + // Segment + segment r_humerus_r_shoulder_elev + parent r_humerus_translation + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0 + 1.0 0.0 0.0 0 + 0 0 0 1 + rotations x + // ranges + // -1.5707963300000001 3.1415926500000002 + endsegment + + // Segment + segment r_humerus_rotation_1 + parent r_humerus_r_shoulder_elev + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + // ranges + // -1.5707963300000001 3.1415926500000002 + endsegment + + // Segment + segment r_humerus_rotation_2 + parent r_humerus_rotation_1 + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + // ranges + // -1.5707963300000001 3.1415926500000002 + endsegment + + + // Segment to cancel transformation bases effect. + // Segment + segment r_humerus_reset_axis + parent r_humerus_rotation_2 + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + endsegment + + + //True segment where are applied inertial values. + // Segment + segment r_humerus + parent r_humerus_reset_axis + RTinMatrix 0 + RT -0.0 0.0 -0.0 xyz -0.0 -0.0 -0.0 + mass 1.8645719999999999 + inertia + 0.01481 0 0 + 0 0.0045510000000000004 0 + 0 0 0.013193 + com 0 -0.18049599999999999 0 + meshfile Geometry/arm_r_humerus.vtp + meshcolor 1 1 1 + meshscale 1 1 1 + endsegment + +// Information about r_ulna_radius_hand segment + // Segment + segment r_ulna_radius_hand_parent_offset + parent r_humerus + RTinMatrix 0 + RT 0 0 0 xyz 0.0061000000000000004 -0.29039999999999999 -0.0123 + endsegment + + // Segments to define transformation axis. + // Segment + segment r_ulna_radius_hand_translation + parent r_ulna_radius_hand_parent_offset + RTinMatrix 1 + RT + 1.0 0.0 0.0 0 + 0.0 1.0 0.0 0 + 0.0 0.0 1.0 0 + 0 0 0 1 + endsegment + + // Segment + segment r_ulna_radius_hand_r_elbow_flex + parent r_ulna_radius_hand_translation + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + rotations x + ranges + -1 2.2689280300000001 + endsegment + + // Segment + segment r_ulna_radius_hand_rotation_1 + parent r_ulna_radius_hand_r_elbow_flex + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + // ranges + // 0 2.2689280300000001 + endsegment + + // Segment + segment r_ulna_radius_hand_rotation_2 + parent r_ulna_radius_hand_rotation_1 + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1 + // ranges + // 0 2.2689280300000001 + endsegment + + + // Segment to cancel transformation bases effect. + // Segment + segment r_ulna_radius_hand_reset_axis + parent r_ulna_radius_hand_rotation_2 + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + endsegment + + // Segment + //True segment where are applied inertial values. + // Segment + segment r_ulna_radius_hand + parent r_ulna_radius_hand_reset_axis + RTinMatrix 0 + RT 0 0 0 xyz 0 0 0 + mass 1.5343150000000001 + inertia + 0.019281 0 0 + 0 0.0015709999999999999 0 + 0 0 0.020062 + com 0 -0.181479 0 + meshfile Geometry/arm_r_ulna.vtp + meshcolor 1 1 1 + meshscale 1 1 1 + endsegment + + marker hand + parent r_ulna_radius_hand + position 0.04 -0.32 0.075 + endmarker + + +//Pedal segment +variables + $wheel_radius 0.1 + $wheel_x_position 0.35 // X position of the wheel + $wheel_y_position 0.0 // Y position of the wheel +endvariables + +segment wheel_rotation + parent r_ulna_radius_hand + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.04 + 0.0 1.0 0.0 -0.32 + 0.0 0.0 1.0 0.075 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ -12*pi 4*pi +endsegment + +segment wheel + parent wheel_rotation + RT 0 0 0 xyz $wheel_radius 0 0 + mass 0.5 + inertia + 0.001254 0 0 + 0 0.001254 0 + 0 0 0.0025 + com 0 0 0 + mesh 0 0 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 +endsegment + + // Contact + contact Wheel_center_contact + parent wheel + position 0 0 0 + axis xy + endcontact + + // Markers + marker wheel_center + parent wheel + position 0 0 0 + endmarker + + marker global_wheel_center + parent base + position $wheel_x_position $wheel_y_position 0 + endmarker + + +// MUSCLE DEFINIION + +// base > r_humerus +musclegroup ground_to_r_humerus + OriginParent base + InsertionParent r_humerus +endmusclegroup + + muscle DeltoideusClavicle_A + type hill + statetype degroote + musclegroup ground_to_r_humerus + OriginPosition -0.016 0.027 0.105 + InsertionPosition 0.0066436613177381703 -0.10980522018450981 0.0011474186050816253 + optimalLength 0.094 + maximalForce 707.70000000000005 + tendonSlackLength 0.087999999999999995 + pennationAngle 0.087266460000000004 + maxVelocity 10 + endmuscle + + viapoint DeltoideusClavicle2-P2 + parent base + muscle DeltoideusClavicle_A + musclegroup ground_to_r_humerus + position 0.023 0.017000000000000001 0.14599999999999999 + endviapoint + + muscle DeltoideusScapula_P + type hill + statetype degroote + musclegroup ground_to_r_humerus + OriginPosition -0.064000000000000001 0.02 0.13 + InsertionPosition -0.0047659122508031749 -0.086162511515571069 0.0062390724151510932 + optimalLength 0.094899999999999998 + maximalForce 1324.4000000000001 + tendonSlackLength 0.075999999999999998 + pennationAngle 0.087266460000000004 + maxVelocity 10 + endmuscle + + viapoint DeltoideusScapulaPost2-P2 + parent base + muscle DeltoideusScapula_P + musclegroup ground_to_r_humerus + position -0.060999999999999999 -0.0050000000000000001 0.16500000000000001 + endviapoint + +// base > r_ulna_radius_hand +musclegroup ground_to_r_ulna_radius_hand + OriginParent base + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlong + type hill + statetype degroote + musclegroup ground_to_r_ulna_radius_hand + OriginPosition -0.042000000000000003 -0.028000000000000001 0.14299999999999999 + InsertionPosition -0.021000000000000001 -0.028000000000000001 0.027 + optimalLength 0.0969 + maximalForce 1580.5999999999999 + tendonSlackLength 0.2412 + pennationAngle 0.17453299999999999 + maxVelocity 10 + endmuscle + + viapoint TRIlong-P2 + parent r_humerus + muscle TRIlong + musclegroup ground_to_r_ulna_radius_hand + position -0.01 -0.29 -0.011 + endviapoint + + muscle BIC_long + type hill + statetype degroote + musclegroup ground_to_r_ulna_radius_hand + OriginPosition -0.029999999999999999 0.01 0.14499999999999999 + InsertionPosition -0.019 -0.059999999999999998 0.027 + optimalLength 0.1421 + maximalForce 485.80000000000001 + tendonSlackLength 0.25679999999999997 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BIC_long-P2 + parent r_humerus + muscle BIC_long + musclegroup ground_to_r_ulna_radius_hand + position 0.011830631116962682 0.02814188158731026 0.020038375639319206 + endviapoint + + muscle BIC_brevis + type hill + statetype degroote + musclegroup ground_to_r_ulna_radius_hand + OriginPosition 0.001 -0.014999999999999999 0.13400000000000001 + InsertionPosition -0.02 -0.059999999999999998 0.029000000000000001 + optimalLength 0.12640000000000001 + maximalForce 693 + tendonSlackLength 0.21199999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle + +/*-------------- WARNINGS--------------- diff --git a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod new file mode 100644 index 0000000..f907b34 --- /dev/null +++ b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod @@ -0,0 +1,388 @@ +version 4 + +// File extracted from /simplified_UL_Seth.osim + +//Publication : Holzbaur, K.R.S., Murray, W.M., Delp, S.L. A Model of the Upper Extremity for Simulating Musculoskeletal Surgery and Analyzing Neuromuscular Control. Annals of Biomedical Engineering, vol 33, pp 829�840, 2005 + +//Credit : The OpenSim Development Team (Reinbolt, J; Seth, A; Habib, A; Hamner, S) adapted from a model originally created by Kate Holzbaur (11/22/04) License: Creative Commons (CCBY 3.0). You are free to distribute, remix, tweak, and build upon this work, even commercially, as long as you credit us for the original creation. http://creativecommons.org/licenses/by/3.0/ + +//Force units : N + +//Length units : meters + + +gravity 0 -9.8065999999999995 0 + +// SEGMENT DEFINITION + +// Information about ground segment + segment base + // meshfile mesh/ground_ribs.vtp + endsegment + +// Information about r_humerus segment + // Segment + segment r_humerus_parent_offset + parent ground + RTinMatrix 0 + RT 0 0 0 xyz -0.017545000000000002 -0.0070000000000000001 0.17000000000000001 + endsegment + + // Segments to define transformation axis. + // Segment + segment r_humerus_translation + parent r_humerus_parent_offset + RTinMatrix 1 + RT + 1.0 0.0 0.0 0 + 0.0 1.0 0.0 0 + 0.0 0.0 1.0 0 + 0 0 0 1 + endsegment + + // Segment + segment r_humerus_r_shoulder_elev + parent r_humerus_translation + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0 + 1.0 0.0 0.0 0 + 0 0 0 1 + rotations x + // ranges + // -1.5707963300000001 3.1415926500000002 + endsegment + + // Segment + segment r_humerus_rotation_1 + parent r_humerus_r_shoulder_elev + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + // ranges + // -1.5707963300000001 3.1415926500000002 + endsegment + + // Segment + segment r_humerus_rotation_2 + parent r_humerus_rotation_1 + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + // ranges + // -1.5707963300000001 3.1415926500000002 + endsegment + + + // Segment to cancel transformation bases effect. + // Segment + segment r_humerus_reset_axis + parent r_humerus_rotation_2 + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + endsegment + + + //True segment where are applied inertial values. + // Segment + segment r_humerus + parent r_humerus_reset_axis + RTinMatrix 0 + RT -0.0 0.0 -0.0 xyz -0.0 -0.0 -0.0 + mass 1.8645719999999999 + inertia + 0.01481 0 0 + 0 0.0045510000000000004 0 + 0 0 0.013193 + com 0 -0.18049599999999999 0 + meshfile Geometry/arm_r_humerus.vtp + meshcolor 1 1 1 + meshscale 1 1 1 + endsegment + +// Information about r_ulna_radius_hand segment + // Segment + segment r_ulna_radius_hand_parent_offset + parent r_humerus + RTinMatrix 0 + RT 0 0 0 xyz 0.0061000000000000004 -0.29039999999999999 -0.0123 + endsegment + + // Segments to define transformation axis. + // Segment + segment r_ulna_radius_hand_translation + parent r_ulna_radius_hand_parent_offset + RTinMatrix 1 + RT + 1.0 0.0 0.0 0 + 0.0 1.0 0.0 0 + 0.0 0.0 1.0 0 + 0 0 0 1 + endsegment + + // Segment + segment r_ulna_radius_hand_r_elbow_flex + parent r_ulna_radius_hand_translation + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + rotations x + // ranges + // 0 2.2689280300000001 + endsegment + + // Segment + segment r_ulna_radius_hand_rotation_1 + parent r_ulna_radius_hand_r_elbow_flex + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + // ranges + // 0 2.2689280300000001 + endsegment + + // Segment + segment r_ulna_radius_hand_rotation_2 + parent r_ulna_radius_hand_rotation_1 + RTinMatrix 1 + RT + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1 + // ranges + // 0 2.2689280300000001 + endsegment + + + // Segment to cancel transformation bases effect. + // Segment + segment r_ulna_radius_hand_reset_axis + parent r_ulna_radius_hand_rotation_2 + RTinMatrix 1 + RT + 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0 + endsegment + + // Segment + //True segment where are applied inertial values. + // Segment + segment r_ulna_radius_hand + parent r_ulna_radius_hand_reset_axis + RTinMatrix 0 + RT 0 0 0 xyz 0 0 0 + mass 1.5343150000000001 + inertia + 0.019281 0 0 + 0 0.0015709999999999999 0 + 0 0 0.020062 + com 0 -0.181479 0 + meshfile Geometry/arm_r_ulna.vtp + meshcolor 1 1 1 + meshscale 1 1 1 + endsegment + + marker hand + parent r_ulna_radius_hand + position 0.04 -0.32 0.075 + endmarker + + +//Pedal segment +variables + $wheel_radius 0.1 + $wheel_x_position 0.35 // X position of the wheel + $wheel_y_position 0.0 // Y position of the wheel +endvariables + +segment wheel_rotation + parent r_ulna_radius_hand + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.04 + 0.0 1.0 0.0 -0.32 + 0.0 0.0 1.0 0.075 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ -12*pi 4*pi +endsegment + +segment wheel + parent wheel_rotation + RT 0 0 0 xyz $wheel_radius 0 0 + mass 1 + inertia + 1 0 0 + 0 1 0 + 0 0 1 + mesh 0 0 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius -0.642787610*$wheel_radius 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius -0.984807753*$wheel_radius 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius -0.866025404*$wheel_radius 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius -0.342020143*$wheel_radius 0 + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0 0 0 + mesh 0.93969262*$wheel_radius 0.342020143*$wheel_radius 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh 0 0 0 + mesh 0.50000000*$wheel_radius 0.866025404*$wheel_radius 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh 0 0 0 + mesh -0.17364818*$wheel_radius 0.984807753*$wheel_radius 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh 0 0 0 + mesh -0.76604444*$wheel_radius 0.642787610*$wheel_radius 0 + mesh -1.00000000*$wheel_radius 0.000000000*$wheel_radius 0 +endsegment + + // Contact + contact Wheel_center_contact + parent wheel + position 0 0 0 + axis xy + endcontact + + // Markers + marker wheel_center + parent wheel + position 0 0 0 + endmarker + + +// MUSCLE DEFINIION + +// base > r_humerus +musclegroup ground_to_r_humerus + OriginParent base + InsertionParent r_humerus +endmusclegroup + + muscle DeltoideusClavicle_A + type hill + statetype degroote + musclegroup ground_to_r_humerus + OriginPosition -0.016 0.027 0.105 + InsertionPosition 0.0066436613177381703 -0.10980522018450981 0.0011474186050816253 + optimalLength 0.094 + maximalForce 707.70000000000005 + tendonSlackLength 0.087999999999999995 + pennationAngle 0.087266460000000004 + maxVelocity 10 + endmuscle + + viapoint DeltoideusClavicle2-P2 + parent base + muscle DeltoideusClavicle_A + musclegroup ground_to_r_humerus + position 0.023 0.017000000000000001 0.14599999999999999 + endviapoint + + muscle DeltoideusScapula_P + type hill + statetype degroote + musclegroup ground_to_r_humerus + OriginPosition -0.064000000000000001 0.02 0.13 + InsertionPosition -0.0047659122508031749 -0.086162511515571069 0.0062390724151510932 + optimalLength 0.094899999999999998 + maximalForce 1324.4000000000001 + tendonSlackLength 0.075999999999999998 + pennationAngle 0.087266460000000004 + maxVelocity 10 + endmuscle + + viapoint DeltoideusScapulaPost2-P2 + parent base + muscle DeltoideusScapula_P + musclegroup ground_to_r_humerus + position -0.060999999999999999 -0.0050000000000000001 0.16500000000000001 + endviapoint + +// base > r_ulna_radius_hand +musclegroup ground_to_r_ulna_radius_hand + OriginParent base + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlong + type hill + statetype degroote + musclegroup ground_to_r_ulna_radius_hand + OriginPosition -0.042000000000000003 -0.028000000000000001 0.14299999999999999 + InsertionPosition -0.021000000000000001 -0.028000000000000001 0.027 + optimalLength 0.0969 + maximalForce 1580.5999999999999 + tendonSlackLength 0.2412 + pennationAngle 0.17453299999999999 + maxVelocity 10 + endmuscle + + viapoint TRIlong-P2 + parent r_humerus + muscle TRIlong + musclegroup ground_to_r_ulna_radius_hand + position -0.01 -0.29 -0.011 + endviapoint + + muscle BIC_long + type hill + statetype degroote + musclegroup ground_to_r_ulna_radius_hand + OriginPosition -0.029999999999999999 0.01 0.14499999999999999 + InsertionPosition -0.019 -0.059999999999999998 0.027 + optimalLength 0.1421 + maximalForce 485.80000000000001 + tendonSlackLength 0.25679999999999997 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BIC_long-P2 + parent r_humerus + muscle BIC_long + musclegroup ground_to_r_ulna_radius_hand + position 0.011830631116962682 0.02814188158731026 0.020038375639319206 + endviapoint + + muscle BIC_brevis + type hill + statetype degroote + musclegroup ground_to_r_ulna_radius_hand + OriginPosition 0.001 -0.014999999999999999 0.13400000000000001 + InsertionPosition -0.02 -0.059999999999999998 0.029000000000000001 + optimalLength 0.12640000000000001 + maximalForce 693 + tendonSlackLength 0.21199999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle + +/*-------------- WARNINGS--------------- diff --git a/cocofest/examples/nmpc/pulse_duration_optimization_musculoskeletal_dynamic_nmpc_cyclic.py b/cocofest/examples/nmpc/pulse_duration_optimization_musculoskeletal_dynamic_nmpc_cyclic.py index 4e292d0..0f8e5b9 100644 --- a/cocofest/examples/nmpc/pulse_duration_optimization_musculoskeletal_dynamic_nmpc_cyclic.py +++ b/cocofest/examples/nmpc/pulse_duration_optimization_musculoskeletal_dynamic_nmpc_cyclic.py @@ -4,58 +4,113 @@ The pulse width between minimal sensitivity threshold and 600us to satisfy the flexion and minimizing required elbow torque control. """ - -import os +import numpy as np import biorbd -from bioptim import Solver +from bioptim import Solver, MultiCyclicNonlinearModelPredictiveControl, Solution, ObjectiveList, ObjectiveFcn, MultiCyclicCycleSolutions from cocofest import ( DingModelPulseWidthFrequencyWithFatigue, NmpcFesMsk, FesMskModel, - SolutionToPickle, - PickleAnimate, ) model = FesMskModel( name=None, biorbd_path="../msk_models/arm26_biceps_1dof.bioMod", muscles_model=[DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIClong")], + stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, 2.9], activate_force_length_relationship=True, activate_force_velocity_relationship=True, + activate_residual_torque=True, + external_force_set=None, ) minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 -nmpc_fes_msk = NmpcFesMsk() +objective_functions = ObjectiveList() +for i in [0, 100]: + objective_functions.add( + ObjectiveFcn.Mayer.MINIMIZE_STATE, + key="q", + weight=100000, + index=[0], + target=np.array([[3.14 / (180 / 5)]]).T, + node=i, + phase=0, + quadratic=True, + ) +for i in [50]: + objective_functions.add( + ObjectiveFcn.Mayer.MINIMIZE_STATE, + key="q", + weight=100000, + index=[0], + target=np.array([[3.14 / (180 / 120)]]).T, + node=i, + phase=0, + quadratic=True, + ) + +nmpc_fes_msk = NmpcFesMsk nmpc = nmpc_fes_msk.prepare_nmpc( model=model, - stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], - cycle_len=100, cycle_duration=1, + n_cycles_to_advance=1, + n_cycles_simultaneous=3, pulse_width={ "min": minimum_pulse_width, "max": 0.0006, "bimapping": False, }, - objective={"minimize_residual_torque": True}, + objective={"minimize_residual_torque": True, + "custom": objective_functions}, msk_info={ - "bound_type": "start_end", - "bound_data": [[5], [120]], + # "bound_type": "start_end", + # "bound_data": [[5], [120]], "with_residual_torque": True, }, + use_sx=False, ) -nmpc_fes_msk.n_cycles = 2 + +n_cycles = 2 + + +def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): + return cycle_idx < n_cycles # True if there are still some cycle to perform + + sol = nmpc.solve( - nmpc_fes_msk.update_functions, + update_functions, solver=Solver.IPOPT(), - cyclic_options={"states": {}}, + cycle_solutions=MultiCyclicCycleSolutions.ALL_CYCLES, get_all_iterations=True, + cyclic_options={"states": {}}, ) biorbd_model = biorbd.Model("../msk_models/arm26_biceps_1dof.bioMod") -temp_pickle_file_path = "pw_optim_dynamic_nmpc_full.pkl" -SolutionToPickle(sol[0], temp_pickle_file_path, "").pickle() +# sol.print_cost() + +# from matplotlib import pyplot as plt +# from bioptim import SolutionMerge + +# solution_time = sol[1][0].decision_time(to_merge=SolutionMerge.KEYS, continuous=True) +# solution_time = [float(j) for j in solution_time] +# solution_time_full = sol[0].decision_time(to_merge=SolutionMerge.KEYS, continuous=True) +# solution_time_full = [float(j) for j in solution_time_full] +# +# plt.plot(solution_time, sol[1][0].decision_states(to_merge=SolutionMerge.NODES)["Cn_BIClong"].squeeze(), label="CN1") +# plt.plot(solution_time, sol[1][0].decision_states(to_merge=SolutionMerge.NODES)["F_BIClong"].squeeze(), label="F1") +# plt.plot(solution_time, sol[1][1].decision_states(to_merge=SolutionMerge.NODES)["Cn_BIClong"].squeeze(), label="CN2") +# plt.plot(solution_time, sol[1][1].decision_states(to_merge=SolutionMerge.NODES)["F_BIClong"].squeeze(), label="F2") +# plt.plot(solution_time_full, sol[0].decision_states(to_merge=SolutionMerge.NODES)["Cn_BIClong"].squeeze(), label="CNfull") +# plt.plot(solution_time_full, sol[0].decision_states(to_merge=SolutionMerge.NODES)["F_BIClong"].squeeze(), label="Ffull") +# plt.legend() +# plt.show() -PickleAnimate(temp_pickle_file_path).animate(model=biorbd_model) +sol[1][0].graphs(show_bounds=True) +sol[1][1].graphs(show_bounds=True) +sol[1][0].animate(n_frames=100) +sol[0].graphs(show_bounds=True) +sol[0].animate(n_frames=100) -os.remove(temp_pickle_file_path) +# sol.graphs(show_bounds=True) +# sol.animate(n_frames=200, show_tracked_markers=True) diff --git a/cocofest/integration/integrator.py b/cocofest/integration/integrator.py new file mode 100644 index 0000000..9d4f25d --- /dev/null +++ b/cocofest/integration/integrator.py @@ -0,0 +1,45 @@ +from bioptim import OdeSolver +from bioptim.dynamics.integrator import RK4 +from casadi import MX, SX, vertcat +import math + + +class ModifiedOdeSolverRK4(OdeSolver.RK4): + def __init__(self, n_integration_steps: int = 20): + super().__init__(n_integration_steps) + + @property + def integrator(self): + return ModifiedRK4 + + +class ModifiedRK4(RK4): + """ + Modified Runge-Kutta method with a customized k4 computation. + """ + + def __init__(self, ode: dict, ode_opt: dict): + """ + Parameters + ---------- + ode: dict + The ode description + ode_opt: dict + The ode options + """ + super(RK4, self).__init__(ode, ode_opt) + + def next_x(self, t0: float | MX | SX, x_prev: MX | SX, u: MX | SX, p: MX | SX, a: MX | SX, d: MX | SX) -> MX | SX: + h = self.h + dt = self.dt + offset = math.exp(-15) + + k1 = self.fun(vertcat(t0, dt), x_prev, self.get_u(u, t0), p, a, d)[:, self.ode_idx] + k2 = self.fun(vertcat(t0 + h / 2, dt), x_prev + h / 2 * k1, self.get_u(u, t0 + h / 2), p, a, d)[:, self.ode_idx] + k3 = self.fun(vertcat(t0 + h / 2, dt), x_prev + h / 2 * k2, self.get_u(u, t0 + h / 2), p, a, d)[:, self.ode_idx] + + # Customize the k4 computation + k4 = self.fun(vertcat(t0 + h-offset, dt), x_prev + h-offset * (k2 + k3) / 2, self.get_u(u, t0 + h-offset), p, a, d)[:, self.ode_idx] + + return x_prev + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4) + diff --git a/cocofest/integration/ivp_fes.py b/cocofest/integration/ivp_fes.py index 8b2740d..093268d 100644 --- a/cocofest/integration/ivp_fes.py +++ b/cocofest/integration/ivp_fes.py @@ -53,7 +53,7 @@ def __init__( model (FesModel type), stim_time (list), pulse_width (float type), pulse_intensity (int | float type), pulse_mode (str type), frequency (int | float type), round_down (bool type) ivp_parameters: dict The parameters for the ivp problem including : - final_time (int | float type), ode_solver (OdeSolver type), use_sx (bool type), n_threads (int type) + final_time (int | float type), ode_solver (OdeSolver type), n_threads (int type) """ self._fill_fes_dict(fes_parameters) @@ -61,8 +61,8 @@ def __init__( self.dictionaries_check() self.model = self.fes_parameters["model"] - self.n_stim = len(self.fes_parameters["stim_time"]) - self.stim_time = self.fes_parameters["stim_time"] + self.stim_time = self.model.stim_time + self.n_stim = len(self.stim_time) self.pulse_width = self.fes_parameters["pulse_width"] self.pulse_intensity = self.fes_parameters["pulse_intensity"] @@ -76,25 +76,10 @@ def __init__( self.pulse_mode = self.fes_parameters["pulse_mode"] self._pulse_mode_settings() - parameters = ParameterList(use_sx=self.ivp_parameters["use_sx"]) + parameters = ParameterList(use_sx=False) parameters_init = InitialGuessList() parameters_bounds = BoundsList() - parameters.add( - name="pulse_apparition_time", - function=DingModelFrequency.set_pulse_apparition_time, - size=self.n_stim, - scaling=VariableScaling("pulse_apparition_time", [1] * self.n_stim), - ) - - parameters_init["pulse_apparition_time"] = np.array(self.stim_time) - parameters_bounds.add( - "pulse_apparition_time", - min_bound=np.array(self.stim_time), - max_bound=np.array(self.stim_time), - interpolation=InterpolationType.CONSTANT, - ) - if isinstance( self.model, DingModelPulseWidthFrequency | DingModelPulseWidthFrequencyWithFatigue, @@ -158,7 +143,7 @@ def __init__( ) = self.build_initial_guess_from_ocp(self) self.ode_solver = self.ivp_parameters["ode_solver"] - self.use_sx = self.ivp_parameters["use_sx"] + self.use_sx = False self.n_threads = self.ivp_parameters["n_threads"] self.fake_ocp = self._prepare_fake_ocp() @@ -185,8 +170,7 @@ def _fill_fes_dict(self, fes_parameters): def _fill_ivp_dict(self, ivp_parameters): default_ivp_dict = { "final_time": None, - "ode_solver": OdeSolver.RK4(n_integration_steps=1), - "use_sx": True, + "ode_solver": OdeSolver.RK1(n_integration_steps=5), "n_threads": 1, } @@ -281,9 +265,6 @@ def dictionaries_check(self): ): raise ValueError("ode_solver must be a OdeSolver type") - if not isinstance(self.ivp_parameters["use_sx"], bool): - raise ValueError("use_sx must be a bool type") - if not isinstance(self.ivp_parameters["n_threads"], int): raise ValueError("n_thread must be a int type") @@ -321,7 +302,7 @@ def _prepare_fake_ocp(self): phase_time=self.final_time, ode_solver=self.ode_solver, control_type=ControlType.CONSTANT, - use_sx=self.use_sx, + use_sx=False, parameters=self.parameters, parameter_init=self.parameters_init, parameter_bounds=self.parameters_bounds, @@ -394,7 +375,7 @@ def from_frequency_and_final_time( model, pulse_width, pulse_intensity, pulse_mode, frequency, round_down ivp_parameters: dict The parameters for the ivp problem including : - final_time, ode_solver, use_sx, n_threads + final_time, ode_solver, n_threads """ frequency = fes_parameters["frequency"] @@ -441,7 +422,7 @@ def from_frequency_and_n_stim( model, n_stim, pulse_width, pulse_intensity, pulse_mode ivp_parameters: dict The parameters for the ivp problem including : - final_time, ode_solver, use_sx, n_threads + final_time, ode_solver, n_threads """ n_stim = fes_parameters["n_stim"] diff --git a/cocofest/models/ding2003.py b/cocofest/models/ding2003.py index a09226f..e058429 100644 --- a/cocofest/models/ding2003.py +++ b/cocofest/models/ding2003.py @@ -1,7 +1,7 @@ from typing import Callable import numpy as np -from casadi import MX, exp, vertcat, if_else +from casadi import MX, exp, vertcat, if_else, logic_and from bioptim import ( ConfigureProblem, @@ -33,6 +33,8 @@ def __init__( self, model_name: str = "ding2003", muscle_name: str = None, + stim_time: list[float] = None, + previous_stim: dict = None, sum_stim_truncation: int = None, is_approximated: bool = False, ): @@ -43,7 +45,9 @@ def __init__( self._with_fatigue = False self.is_approximated = is_approximated self.pulse_apparition_time = None - self.stim_prev = [] + self.stim_time = stim_time + self.previous_stim = previous_stim if previous_stim else {"time": []} + self.all_stim = self.previous_stim["time"] + self.stim_time if self.previous_stim else self.stim_time # --- Default values --- # TAUC_DEFAULT = 0.020 # Value from Ding's experimentation [1] (s) @@ -153,10 +157,11 @@ def system_dynamics( cn: MX, f: MX, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, + t_stim_prev: list[MX] = None, cn_sum: MX | float = None, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, + passive_force_relationship: MX | float = 0, ) -> MX: """ The system dynamics is the function that describes the models. @@ -169,7 +174,7 @@ def system_dynamics( 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] + t_stim_prev: list[MX] The time list of the previous stimulations (s) cn_sum: MX | float The sum of the ca_troponin_complex (unitless) @@ -177,6 +182,8 @@ def system_dynamics( 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 ------- @@ -191,6 +198,7 @@ def system_dynamics( 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) @@ -199,9 +207,9 @@ def exp_time_fun(self, t: MX, t_stim_i: MX) -> MX | float: Parameters ---------- t: MX - The current time at which the dynamics is evaluated (ms) + The current time at which the dynamics is evaluated (s) t_stim_i: MX - Time when the stimulation i occurred (ms) + Time when the stimulation i occurred (s) Returns ------- @@ -216,7 +224,7 @@ def ri_fun(self, r0: MX | float, time_between_stim: MX) -> MX | float: r0: MX | float Mathematical term characterizing the magnitude of enhancement in CN from the following stimuli (unitless) time_between_stim: MX - Time between the last stimulation i and the current stimulation i (ms) + Time between the last stimulation i and the current stimulation i (s) Returns ------- @@ -231,9 +239,9 @@ def cn_sum_fun(self, r0: MX | float, t: MX, t_stim_prev: list[MX], lambda_i: lis r0: MX | float Mathematical term characterizing the magnitude of enhancement in CN from the following stimuli (unitless) t: MX - The current time at which the dynamics is evaluated (ms) + The current time at which the dynamics is evaluated (s) t_stim_prev: list[MX] - The time list of the previous stimulations (ms) + The time list of the previous stimulations (s) Returns ------- @@ -241,12 +249,11 @@ def cn_sum_fun(self, r0: MX | float, t: MX, t_stim_prev: list[MX], lambda_i: lis """ sum_multiplier = 0 - for i in range(len(t_stim_prev)): + for i in range(t_stim_prev.shape[0]): previous_phase_time = t_stim_prev[i] - t_stim_prev[i - 1] ri = 1 if i == 0 else self.ri_fun(r0, previous_phase_time) # Part of Eq n°1 exp_time = self.exp_time_fun(t, t_stim_prev[i]) # Part of Eq n°1 - coefficient = 1 if self.is_approximated else if_else(t_stim_prev[i] <= t, 1, 0) - sum_multiplier += ri * exp_time * lambda_i[i] * coefficient + sum_multiplier += ri * exp_time * lambda_i[i] return sum_multiplier def cn_dot_fun(self, cn: MX, cn_sum: MX) -> MX | float: @@ -268,7 +275,7 @@ def calculate_cn_dot(self, cn, cn_sum, t, t_stim_prev, pulse_intensity=1): return self.cn_dot_fun(cn=cn, cn_sum=cn_sum) else: cn_sum = self.cn_sum_fun( - self.get_r0(self.km_rest), t, t_stim_prev, self.get_lambda_i(len(t_stim_prev), pulse_intensity) + self.get_r0(self.km_rest), t, t_stim_prev, self.get_lambda_i(t_stim_prev.shape[0], pulse_intensity) ) return self.cn_dot_fun(cn, cn_sum) @@ -281,6 +288,7 @@ def f_dot_fun( km: MX | float, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, + passive_force_relationship: MX | float = 0, ) -> MX | float: """ Parameters @@ -292,13 +300,15 @@ def f_dot_fun( a: MX | float The previous step value of scaling factor (unitless) tau1: MX | float - The previous step value of time_state_force_no_cross_bridge (ms) + 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 ------- @@ -306,8 +316,9 @@ def f_dot_fun( """ return ( (a * (cn / (km + cn)) - (f / (tau1 + self.tau2 * (cn / (km + cn))))) - * force_length_relationship + * (force_length_relationship * force_velocity_relationship + + passive_force_relationship) ) # Equation n°2 @staticmethod @@ -322,6 +333,7 @@ def dynamics( 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 @@ -348,29 +360,29 @@ def dynamics( 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 - stim_apparition = None cn_sum = None if model.is_approximated: cn_sum = controls[0] - else: - stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) return DynamicsEvaluation( dxdt=dxdt_fun( cn=states[0], f=states[1], t=time, - t_stim_prev=stim_apparition, + t_stim_prev=numerical_timeseries, cn_sum=cn_sum, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, + passive_force_relationship=passive_force_relationship, ), ) @@ -393,39 +405,7 @@ def declare_ding_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) + StateConfigure().configure_last_pulse_width(ocp, nlp) if self.is_approximated: StateConfigure().configure_cn_sum(ocp, nlp) ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) - - @staticmethod - def get_stim(nlp: NonLinearProgram, parameters: MX) -> list[float]: - """ - Get the nlp list of previous stimulation apparition time - - Parameters - ---------- - nlp: NonLinearProgram - The NonLinearProgram of the ocp of the current phase - parameters: MX - The parameters of the ocp - - Returns - ------- - The list of stimulation time - """ - t_stim = [] - for j in range(parameters.shape[0]): - if "pulse_apparition_time" in nlp.parameters.cx[j].str(): - t_stim.append(parameters[j]) - return t_stim - - def set_pulse_apparition_time(self, value: list[MX]): - """ - Sets the pulse apparition time for each pulse (phases) according to the ocp parameter "pulse_apparition_time" - - Parameters - ---------- - value: list[MX] - The pulse apparition time list (s) - """ - self.pulse_apparition_time = value diff --git a/cocofest/models/ding2003_with_fatigue.py b/cocofest/models/ding2003_with_fatigue.py index 2217e46..a91ac1a 100644 --- a/cocofest/models/ding2003_with_fatigue.py +++ b/cocofest/models/ding2003_with_fatigue.py @@ -31,22 +31,26 @@ def __init__( self, model_name: str = "ding2003_with_fatigue", muscle_name: str = None, + stim_time: list[float] = None, + previous_stim: dict = None, sum_stim_truncation: int = None, is_approximated: bool = False, ): super().__init__( model_name=model_name, muscle_name=muscle_name, + stim_time=stim_time, + previous_stim=previous_stim, sum_stim_truncation=sum_stim_truncation, ) self._with_fatigue = True self.is_approximated = is_approximated # --- Default values --- # - ALPHA_A_DEFAULT = -4.0 * 10e-7 # Value from Ding's experimentation [1] (s^-2) - ALPHA_TAU1_DEFAULT = 2.1 * 10e-5 # Value from Ding's experimentation [1] (N^-1) + ALPHA_A_DEFAULT = -4.0 * 10e-2 # Value from Ding's experimentation [1] (s^-2) TAU_FAT_DEFAULT = 127 # Value from Ding's experimentation [1] (s) - ALPHA_KM_DEFAULT = 1.9 * 10e-8 # Value from Ding's experimentation [1] (s^-1.N^-1) + ALPHA_TAU1_DEFAULT = 2.1 * 10e-6 # Value from Ding's experimentation [1] (N^-1) + ALPHA_KM_DEFAULT = 1.9 * 10e-6 # Value from Ding's experimentation [1] (s^-1.N^-1) # ---- Fatigue models ---- # self.alpha_a = ALPHA_A_DEFAULT @@ -139,10 +143,11 @@ def system_dynamics( tau1: MX = None, km: MX = None, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, + t_stim_prev: MX | float = None, cn_sum: MX | float = None, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, + passive_force_relationship: MX | float = 0, ) -> MX: """ The system dynamics is the function that describes the models. @@ -156,19 +161,21 @@ def system_dynamics( a: MX The value of the scaling factor (unitless) tau1: MX - The value of the time_state_force_no_cross_bridge (ms) + 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: list[MX] | list[float] - The time list of the previous stimulations (s) + t_stim_prev: MX | float + The previous time at which the stimulation was applied (s) cn_sum: MX | float The sum of the ca_troponin_complex (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 ------- @@ -183,6 +190,7 @@ def system_dynamics( 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 @@ -209,13 +217,13 @@ def tau1_dot_fun(self, tau1: MX, f: MX) -> MX | float: Parameters ---------- tau1: MX - The previous step value of time_state_force_no_cross_bridge (ms) + The previous step value of time_state_force_no_cross_bridge (s) f: MX The previous step value of force (N) Returns ------- - The value of the derivative time_state_force_no_cross_bridge (ms) + The value of the derivative time_state_force_no_cross_bridge (s) """ return -(tau1 - self.tau1_rest) / self.tau_fat + self.alpha_tau1 * f # Equation n°9 @@ -246,6 +254,7 @@ def dynamics( 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 @@ -272,19 +281,18 @@ def dynamics( 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 - stim_apparition = None cn_sum = None if model.is_approximated: cn_sum = controls[0] - else: - stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) return DynamicsEvaluation( dxdt=dxdt_fun( @@ -295,9 +303,10 @@ def dynamics( km=states[4], cn_sum=cn_sum, t=time, - t_stim_prev=stim_apparition, + 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, ) @@ -321,6 +330,7 @@ def declare_ding_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) + StateConfigure().configure_last_pulse_width(ocp, nlp) if self.is_approximated: StateConfigure().configure_cn_sum(ocp, nlp) ConfigureProblem.configure_dynamics_function(ocp, nlp, dyn_func=self.dynamics) diff --git a/cocofest/models/ding2007.py b/cocofest/models/ding2007.py index 4adaf81..eef7718 100644 --- a/cocofest/models/ding2007.py +++ b/cocofest/models/ding2007.py @@ -31,6 +31,8 @@ def __init__( self, model_name: str = "ding_2007", muscle_name: str = None, + stim_time: list[float] = None, + previous_stim: dict = None, sum_stim_truncation: int = None, is_approximated: bool = False, tauc: float = None, @@ -49,11 +51,16 @@ def __init__( super(DingModelPulseWidthFrequency, self).__init__( model_name=model_name, muscle_name=muscle_name, + stim_time=stim_time, + previous_stim=previous_stim, sum_stim_truncation=sum_stim_truncation, is_approximated=is_approximated, ) self._with_fatigue = False self.pulse_width = None + self.stim_time = stim_time + self.previous_stim = previous_stim if previous_stim else {"time": [], "pulse_width": []} + self.all_stim = previous_stim["time"] + stim_time if previous_stim else stim_time # --- Default values --- # A_SCALE_DEFAULT = 4920 # Value from Ding's 2007 article (N/s) @@ -109,6 +116,8 @@ def serialize(self) -> tuple[Callable, dict]: "a_scale": self.a_scale, "pd0": self.pd0, "pdt": self.pdt, + "stim_time": self.stim_time, + "previous_stim": self.previous_stim, }, ) @@ -117,12 +126,13 @@ def system_dynamics( cn: MX, f: MX, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, + t_stim_prev: list[float] | list[MX] = None, pulse_width: MX = None, cn_sum: MX = None, a_scale: MX = None, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, + passive_force_relationship: MX | float = 0, ) -> MX: """ The system dynamics is the function that describes the models. @@ -135,7 +145,7 @@ def system_dynamics( 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] + 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) @@ -147,6 +157,8 @@ def system_dynamics( 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 ------- @@ -159,8 +171,6 @@ def system_dynamics( else self.a_calculation( a_scale=self.a_scale, pulse_width=pulse_width, - t=t, - t_stim_prev=t_stim_prev, ) ) @@ -172,6 +182,7 @@ def system_dynamics( 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) @@ -179,8 +190,6 @@ def a_calculation( self, a_scale: float | MX, pulse_width: MX, - t=None, - t_stim_prev: list[float] | list[MX] = None, ) -> MX: """ Parameters @@ -189,26 +198,11 @@ def a_calculation( The scaling factor of the current stimulation (unitless) pulse_width: MX The pulsation duration of the current stimulation (s) - 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) Returns ------- The value of scaling factor (unitless) """ - if self.is_approximated: - return a_scale * (1 - exp(-(pulse_width - self.pd0) / self.pdt)) - else: - pulse_width_list = pulse_width - for i in range(len(t_stim_prev)): - if i == 0: - pulse_width = pulse_width_list[0] - else: - coefficient = if_else(t_stim_prev[i] <= t, 1, 0) - temp_pulse_width = pulse_width_list[i] * coefficient - pulse_width = if_else(temp_pulse_width != 0, temp_pulse_width, pulse_width) - return a_scale * (1 - exp(-(pulse_width - self.pd0) / self.pdt)) + return a_scale * (1 - exp(-(pulse_width - self.pd0) / self.pdt)) def a_calculation_identification( self, @@ -268,7 +262,7 @@ def get_pulse_width_parameters(nlp, parameters: ParameterList, muscle_name: str pulse_width_parameters = [] for j in range(parameters.shape[0]): if muscle_name: - if "pulse_width_" + muscle_name in nlp.parameters.scaled.cx[j].str(): + if "pulse_width_" + muscle_name in parameters[j].str(): pulse_width_parameters.append(parameters[j]) elif "pulse_width" in nlp.parameters.scaled.cx[j].str(): pulse_width_parameters.append(parameters[j]) @@ -286,6 +280,7 @@ def dynamics( 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 @@ -312,6 +307,8 @@ def dynamics( 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 @@ -319,31 +316,24 @@ def dynamics( model = fes_model if fes_model else nlp.model dxdt_fun = model.system_dynamics + cn_sum = None + a_scale = None if model.is_approximated: - pulse_width = None - stim_apparition = None cn_sum = controls[0] a_scale = controls[1] - else: - pulse_width = model.get_pulse_width_parameters(nlp, parameters) - stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) - - if len(pulse_width) == 1 and len(stim_apparition) != 1: - pulse_width = pulse_width * len(stim_apparition) - cn_sum = None - a_scale = None return DynamicsEvaluation( dxdt=dxdt_fun( cn=states[0], f=states[1], t=time, - t_stim_prev=stim_apparition, - pulse_width=pulse_width, + t_stim_prev=numerical_timeseries, + pulse_width=controls[0], cn_sum=cn_sum, a_scale=a_scale, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, + passive_force_relationship=passive_force_relationship ), defects=None, ) @@ -367,6 +357,7 @@ def declare_ding_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) + StateConfigure().configure_last_pulse_width(ocp, nlp) if self.is_approximated: StateConfigure().configure_cn_sum(ocp, nlp) StateConfigure().configure_a_calculation(ocp, nlp) diff --git a/cocofest/models/ding2007_with_fatigue.py b/cocofest/models/ding2007_with_fatigue.py index 8dd6e2c..12ff5f9 100644 --- a/cocofest/models/ding2007_with_fatigue.py +++ b/cocofest/models/ding2007_with_fatigue.py @@ -30,6 +30,8 @@ def __init__( self, model_name: str = "ding_2007_with_fatigue", muscle_name: str = None, + stim_time: list[float] = None, + previous_stim: dict = None, sum_stim_truncation: int = None, is_approximated: bool = False, tauc: float = None, @@ -45,19 +47,25 @@ def __init__( alpha_km: float = None, tau_fat: float = None, ): + if previous_stim: + if len(previous_stim["time"]) != len(previous_stim["pulse_width"]): + raise ValueError("The previous_stim time and pulse_width must have the same length") super(DingModelPulseWidthFrequencyWithFatigue, self).__init__( model_name=model_name, muscle_name=muscle_name, + stim_time=stim_time, + previous_stim=previous_stim, sum_stim_truncation=sum_stim_truncation, is_approximated=is_approximated, ) self._with_fatigue = True + self.stim_time = stim_time # --- Default values --- # - ALPHA_A_DEFAULT = -4.0 * 10e-7 # Value from Ding's experimentation [1] (s^-2) - ALPHA_TAU1_DEFAULT = 2.1 * 10e-5 # Value from Ding's experimentation [1] (N^-1) + ALPHA_A_DEFAULT = -4.0 * 10e-2 # Value from Ding's experimentation [1] (s^-2) TAU_FAT_DEFAULT = 127 # Value from Ding's experimentation [1] (s) - ALPHA_KM_DEFAULT = 1.9 * 10e-8 # Value from Ding's experimentation [1] (s^-1.N^-1) + ALPHA_TAU1_DEFAULT = 2.1 * 10e-6 # Value from Ding's experimentation [1] (N^-1) + ALPHA_KM_DEFAULT = 1.9 * 10e-6 # Value from Ding's experimentation [1] (s^-1.N^-1) # ---- Fatigue models ---- # self.alpha_a = ALPHA_A_DEFAULT @@ -122,6 +130,8 @@ def serialize(self) -> tuple[Callable, dict]: "a_scale": self.a_scale, "pd0": self.pd0, "pdt": self.pdt, + "stim_time": self.stim_time, + "previous_stim": self.previous_stim, }, ) @@ -133,12 +143,13 @@ def system_dynamics( tau1: MX = None, km: MX = None, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, + t_stim_prev: MX = None, pulse_width: MX = None, cn_sum: MX = None, a_scale: MX = None, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, + passive_force_relationship: MX | float = 0, ) -> MX: """ The system dynamics is the function that describes the models. @@ -157,19 +168,20 @@ def system_dynamics( The value of the cross_bridges (unitless) t: MX The current time at which the dynamics is evaluated (s) - t_stim_prev: list[MX] | list[float] - The time list of the previous stimulations (s) + t_stim_prev: MX + The time of the previous stimulation (s) pulse_width: MX The time of the impulse (s) cn_sum: MX | float The sum of the ca_troponin_complex (unitless) a_scale: MX | float The scaling factor (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 ------- @@ -180,10 +192,8 @@ def system_dynamics( a_scale if self.is_approximated else self.a_calculation( - a_scale=self.a_scale, + a_scale=a, pulse_width=pulse_width, - t=t, - t_stim_prev=t_stim_prev, ) ) @@ -195,6 +205,7 @@ def system_dynamics( 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) @@ -259,6 +270,7 @@ def dynamics( 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 @@ -285,6 +297,8 @@ def dynamics( 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 @@ -292,25 +306,11 @@ def dynamics( model = fes_model if fes_model else nlp.model dxdt_fun = model.system_dynamics + cn_sum = None + a_scale = None if model.is_approximated: - cn_sum = controls[0] - a_scale = controls[1] - stim_apparition = None - pulse_width = None - else: - pulse_width = ( - model.get_pulse_width_parameters(nlp, parameters) - if fes_model is None - else fes_model.get_pulse_width_parameters(nlp, parameters, muscle_name=fes_model.muscle_name) - ) - - stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) - - if len(pulse_width) == 1 and len(stim_apparition) != 1: - pulse_width = pulse_width * len(stim_apparition) - - cn_sum = None - a_scale = None + cn_sum = controls[1] + a_scale = controls[2] return DynamicsEvaluation( dxdt=dxdt_fun( @@ -320,12 +320,13 @@ def dynamics( tau1=states[3], km=states[4], t=time, - t_stim_prev=stim_apparition, - pulse_width=pulse_width, + t_stim_prev=numerical_timeseries, + pulse_width=controls[0], cn_sum=cn_sum, a_scale=a_scale, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, + passive_force_relationship=passive_force_relationship ), defects=None, ) @@ -349,6 +350,7 @@ def declare_ding_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ StateConfigure().configure_all_fes_model_states(ocp, nlp, fes_model=self) + StateConfigure().configure_last_pulse_width(ocp, nlp) if self.is_approximated: StateConfigure().configure_cn_sum(ocp, nlp) StateConfigure().configure_a_calculation(ocp, nlp) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 86bc8bb..f5cb417 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -4,6 +4,7 @@ from casadi import vertcat, MX, SX, Function from bioptim import ( BiorbdModel, + ExternalForceSetTimeSeries, OptimalControlProgram, NonLinearProgram, ConfigureProblem, @@ -18,6 +19,7 @@ from .hill_coefficients import ( muscle_force_length_coefficient, muscle_force_velocity_coefficient, + muscle_passive_force_coefficient, ) @@ -27,10 +29,14 @@ def __init__( name: str = None, biorbd_path: str = None, muscles_model: list[FesModel] = None, + stim_time: list[float] = None, + previous_stim: dict = None, activate_force_length_relationship: bool = False, activate_force_velocity_relationship: bool = False, + activate_passive_force_relationship: bool = False, activate_residual_torque: bool = False, parameters: ParameterList = None, + external_force_set: ExternalForceSetTimeSeries = None, ): """ The custom model that will be used in the optimal control program for the FES-MSK models @@ -47,14 +53,16 @@ def __init__( If the force-length relationship should be activated activate_force_velocity_relationship: bool If the force-velocity relationship should be activated + activate_passive_force_relationship: bool + If the passive force relationship should be activated activate_residual_torque: bool If the residual torque should be activated parameters: ParameterList The parameters that will be used in the model """ - super().__init__(biorbd_path, parameters=parameters) + 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) self._name = name - self.bio_model = BiorbdModel(biorbd_path, parameters=parameters) self.biorbd_path = biorbd_path self._model_sanity( @@ -63,11 +71,19 @@ def __init__( activate_force_velocity_relationship, ) self.muscles_dynamics_model = muscles_model + for i in range(len(self.muscles_dynamics_model)): + self.muscles_dynamics_model[i].stim_time = stim_time + self.muscles_dynamics_model[i].previous_stim = previous_stim + self.muscles_dynamics_model[i].all_stim = previous_stim["time"] + stim_time if previous_stim else stim_time + self.bio_stim_model = [self.bio_model] + 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 # ---- Absolutely needed methods ---- # def serialize(self) -> tuple[Callable, dict]: @@ -77,8 +93,14 @@ def serialize(self) -> tuple[Callable, dict]: "name": self._name, "biorbd_path": self.bio_model.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, "activate_force_length_relationship": self.activate_force_length_relationship, "activate_force_velocity_relationship": self.activate_force_velocity_relationship, + "activate_passive_force_relationship": self.activate_passive_force_relationship, + "activate_residual_torque": self.activate_residual_torque, + "parameters": self.parameters_list, + "external_force_set": self.external_forces_set, }, ) @@ -163,7 +185,9 @@ def muscle_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau - ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, total_torque, [], parameters) + 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)(q, qdot, total_torque, external_forces, parameters) # q, qdot, tau, external_forces, parameters dxdt = vertcat(dxdt_muscle_list, dq, ddq) return DynamicsEvaluation(dxdt=dxdt, defects=None) @@ -207,6 +231,10 @@ def muscles_joint_torque( ] muscle_states = vertcat(*[states[i] for i in muscle_states_idxs]) + muscle_parameters_idxs = [ + i for i in range(parameters.shape[0]) if muscle_model.muscle_name in str(parameters[i]) + ] + muscle_parameters = vertcat(*[parameters[i] for i in muscle_parameters_idxs]) muscle_idx = bio_muscle_names_at_index.index(muscle_model.muscle_name) @@ -237,17 +265,31 @@ def muscles_joint_torque( "muscle_force_velocity_coeff", [Q, Qdot], [muscle_force_velocity_coeff] )(q, qdot) + muscle_passive_force_coeff = ( + muscle_passive_force_coefficient( + model=updatedModel, + muscle=nlp.model.bio_model.model.muscle(muscle_idx), + q=Q, + ) + if nlp.model.activate_passive_force_relationship + else 0 + ) + muscle_passive_force_coeff = Function( + "muscle_passive_force_coeff", [Q, Qdot], [muscle_passive_force_coeff] + )(q, qdot) + muscle_dxdt = muscle_model.dynamics( time, muscle_states, controls, - parameters, + muscle_parameters, algebraic_states, 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 dxdt_muscle_list = vertcat(dxdt_muscle_list, muscle_dxdt) @@ -265,10 +307,83 @@ def muscles_joint_torque( return muscle_joint_torques, dxdt_muscle_list + @staticmethod + def forces_from_fes_driven( + time: MX.sym, + states: MX.sym, + controls: MX.sym, + parameters: MX.sym, + algebraic_states: MX.sym, + numerical_timeseries: MX.sym, + nlp, + with_passive_torque: bool = False, + with_ligament: bool = False, + state_name_list=None, + ) -> MX: + """ + Contact forces of a forward dynamics driven by muscles activations and joint torques with contact constraints. + + Parameters + ---------- + time: MX.sym + The time of the system + states: MX.sym + The state of the system + controls: MX.sym + The controls of the system + parameters: MX.sym + The parameters of the system + algebraic_states: MX.sym + The algebraic states of the system + numerical_timeseries: MX.sym + The numerical timeseries of the system + nlp: NonLinearProgram + The definition of the system + with_passive_torque: bool + If the dynamic with passive torque should be used + with_ligament: bool + If the dynamic with ligament should be used + state_name_list: list[str] + The states names list + Returns + ---------- + MX.sym + The contact forces that ensure no acceleration at these contact points + """ + + q = nlp.get_var_from_states_or_controls("q", states, controls) + qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) + residual_tau = nlp.get_var_from_states_or_controls("tau", states, controls) if "tau" in nlp.controls else None + # mus_activations = nlp.get_var_from_states_or_controls("muscles", states, controls) + # muscles_tau = DynamicsFunctions.compute_tau_from_muscle(nlp, q, qdot, mus_activations) + + muscles_joint_torque, _ = FesMskModel.muscles_joint_torque( + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, + nlp, + nlp.model.muscles_dynamics_model, + state_name_list, + q, + qdot, + ) + + tau = muscles_joint_torque + residual_tau if residual_tau is not None else muscles_joint_torque + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) if with_ligament else tau + + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) + def declare_model_variables( self, ocp: OptimalControlProgram, nlp: NonLinearProgram, + with_contact: bool = False, numerical_data_timeseries: dict[str, np.ndarray] = None, ): """ @@ -284,6 +399,7 @@ def declare_model_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ + 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") @@ -304,6 +420,11 @@ def declare_model_variables( state_name_list=state_name_list, ) + if with_contact: + ConfigureProblem.configure_contact_function( + ocp, nlp, self.forces_from_fes_driven, state_name_list=state_name_list + ) + @staticmethod def _model_sanity( muscles_model, @@ -327,3 +448,19 @@ def _model_sanity( if not isinstance(activate_force_velocity_relationship, bool): raise TypeError("The activate_force_velocity_relationship must be a boolean") + + +def _check_numerical_timeseries_format(numerical_timeseries: np.ndarray, n_shooting: int, phase_idx: int): + """Check if the numerical_data_timeseries is of the right format""" + if type(numerical_timeseries) is not np.ndarray: + raise RuntimeError( + f"Phase {phase_idx} has numerical_data_timeseries of type {type(numerical_timeseries)} " + f"but it should be of type np.ndarray" + ) + if numerical_timeseries is not None and numerical_timeseries.shape[2] != n_shooting + 1: + raise RuntimeError( + f"Phase {phase_idx} has {n_shooting}+1 shooting points but the numerical_data_timeseries " + f"has {numerical_timeseries.shape[2]} shooting points." + 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 " + ) \ No newline at end of file diff --git a/cocofest/models/fes_model.py b/cocofest/models/fes_model.py index b1ab740..5476678 100644 --- a/cocofest/models/fes_model.py +++ b/cocofest/models/fes_model.py @@ -113,6 +113,7 @@ def system_dynamics( cn_sum: MX, force_length_relationship: MX | float, force_velocity_relationship: MX | float, + passive_force_relationship: MX | float, ): """ @@ -166,6 +167,7 @@ def f_dot_fun( km: MX | float, force_length_relationship: MX | float, force_velocity_relationship: MX | float, + passive_force_relationship: MX | float, ): """ @@ -187,6 +189,7 @@ def dynamics( fes_model, force_length_relationship: MX | float, force_velocity_relationship: MX | float, + passive_force_relationship: MX | float, ): """ @@ -209,11 +212,3 @@ def declare_ding_variables( """ - @abstractmethod - def set_pulse_apparition_time(self, value: list[MX]): - """ - - Returns - ------- - - """ diff --git a/cocofest/models/hill_coefficients.py b/cocofest/models/hill_coefficients.py index 6229b4c..8298d7d 100644 --- a/cocofest/models/hill_coefficients.py +++ b/cocofest/models/hill_coefficients.py @@ -1,3 +1,10 @@ +""" +The muscle force length, velocity and passive force coefficients calculation from: +De Groote, F., Kinney, A. L., Rao, A. V., & Fregly, B. J. (2016). +Evaluation of direct collocation optimal control problem formulations for solving the muscle redundancy problem. +Annals of biomedical engineering, 44, 2922-2936. +""" + from casadi import exp, log, sqrt @@ -87,3 +94,33 @@ def muscle_force_velocity_coefficient(model, muscle, q, qdot): m_FvCE = d1 * log((d2 * norm_v + d3) + sqrt((d2 * norm_v + d3) * (d2 * norm_v + d3) + 1)) + d4 return m_FvCE + + +def muscle_passive_force_coefficient(model, muscle, q): + """ + Muscle passive force coefficient from HillDeGroote + + Parameters + ---------- + model: BiorbdModel + The biorbd model + muscle: MX + The muscle + q: MX + The generalized coordinates + + Returns + ------- + The muscle passive force coefficient + """ + kpe = 4 + e0 = 0.6 + + muscle_length = muscle.length(model, q, False).to_mx() + muscle_optimal_length = muscle.characteristics().optimalLength().to_mx() + norm_length = muscle_length / muscle_optimal_length + + m_FpCE = (exp(kpe * (norm_length - 1) / e0) - 1) / (exp(kpe) - 1) + m_FpCE = m_FpCE if m_FpCE > 0 else 0 + + return m_FpCE diff --git a/cocofest/models/hmed2018.py b/cocofest/models/hmed2018.py index 3fb1d55..01921b4 100644 --- a/cocofest/models/hmed2018.py +++ b/cocofest/models/hmed2018.py @@ -31,12 +31,19 @@ def __init__( self, model_name: str = "hmed2018", muscle_name: str = None, + stim_time: list[float] = None, + previous_stim: dict = None, sum_stim_truncation: int = None, is_approximated: bool = False, ): + if previous_stim: + if len(previous_stim["time"]) != len(previous_stim["pulse_intensity"]): + raise ValueError("The previous_stim time and pulse_intensity must have the same length") super(DingModelPulseIntensityFrequency, self).__init__( model_name=model_name, muscle_name=muscle_name, + stim_time=stim_time, + previous_stim=previous_stim, sum_stim_truncation=sum_stim_truncation, is_approximated=is_approximated, ) @@ -115,11 +122,11 @@ def system_dynamics( cn: MX, f: MX, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, pulse_intensity: list[MX] | list[float] = None, cn_sum: MX = None, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, + passive_force_relationship: MX | float = 0, ) -> MX: """ The system dynamics is the function that describes the models. @@ -132,8 +139,6 @@ def system_dynamics( 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 time list of the previous stimulations (s) pulse_intensity: list[MX] | list[float] The pulsation intensity of the current stimulation (mA) cn_sum: MX | float @@ -142,11 +147,16 @@ def system_dynamics( 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 of each state dx/dt at the current time t """ + t_stim_prev = self.all_stim + if self.all_stim != self.stim_time: + pulse_intensity = self.previous_stim["pulse_intensity"] + pulse_intensity cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev, pulse_intensity) f_dot = self.f_dot_fun( cn, @@ -156,6 +166,7 @@ def system_dynamics( 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) @@ -221,6 +232,7 @@ def dynamics( 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 @@ -247,6 +259,8 @@ def dynamics( 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 @@ -256,26 +270,24 @@ def dynamics( if model.is_approximated: cn_sum = controls[0] - stim_apparition = None intensity_parameters = None else: cn_sum = None intensity_parameters = model.get_intensity_parameters(nlp, parameters) - stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) - if len(intensity_parameters) == 1 and len(stim_apparition) != 1: - intensity_parameters = intensity_parameters * len(stim_apparition) + if len(intensity_parameters) == 1 and len(nlp.model.stim_time) != 1: + intensity_parameters = intensity_parameters * len(nlp.model.stim_time) return DynamicsEvaluation( dxdt=dxdt_fun( cn=states[0], f=states[1], t=time, - t_stim_prev=stim_apparition, pulse_intensity=intensity_parameters, cn_sum=cn_sum, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, + passive_force_relationship=passive_force_relationship, ), defects=None, ) diff --git a/cocofest/models/hmed2018_with_fatigue.py b/cocofest/models/hmed2018_with_fatigue.py index 321ee56..dec0cfd 100644 --- a/cocofest/models/hmed2018_with_fatigue.py +++ b/cocofest/models/hmed2018_with_fatigue.py @@ -30,22 +30,29 @@ def __init__( self, model_name: str = "hmed2018_with_fatigue", muscle_name: str = None, + stim_time: list[float] = None, + previous_stim: dict = None, sum_stim_truncation: int = None, is_approximated: bool = False, ): + if previous_stim: + if len(previous_stim["time"]) != len(previous_stim["pulse_intensity"]): + raise ValueError("The previous_stim time and pulse_intensity must have the same length") super(DingModelPulseIntensityFrequencyWithFatigue, self).__init__( model_name=model_name, muscle_name=muscle_name, + stim_time=stim_time, + previous_stim=previous_stim, sum_stim_truncation=sum_stim_truncation, is_approximated=is_approximated, ) self._with_fatigue = True # --- Default values --- # - ALPHA_A_DEFAULT = -4.0 * 10e-7 # Value from Ding's experimentation [1] (s^-2) - ALPHA_TAU1_DEFAULT = 2.1 * 10e-5 # Value from Ding's experimentation [1] (N^-1) + ALPHA_A_DEFAULT = -4.0 * 10e-2 # Value from Ding's experimentation [1] (s^-2) TAU_FAT_DEFAULT = 127 # Value from Ding's experimentation [1] (s) - ALPHA_KM_DEFAULT = 1.9 * 10e-8 # Value from Ding's experimentation [1] (s^-1.N^-1) + ALPHA_TAU1_DEFAULT = 2.1 * 10e-6 # Value from Ding's experimentation [1] (N^-1) + ALPHA_KM_DEFAULT = 1.9 * 10e-6 # Value from Ding's experimentation [1] (s^-1.N^-1) # ---- Fatigue models ---- # self.alpha_a = ALPHA_A_DEFAULT @@ -124,11 +131,11 @@ def system_dynamics( tau1: MX = None, km: MX = None, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, pulse_intensity: list[MX] | list[float] = None, cn_sum: MX = None, force_length_relationship: float | MX = 1, force_velocity_relationship: float | MX = 1, + passive_force_relationship: MX | float = 0, ) -> MX: """ The system dynamics is the function that describes the models. @@ -147,8 +154,6 @@ def system_dynamics( The value of the cross_bridges (unitless) t: MX The current time at which the dynamics is evaluated (s) - t_stim_prev: list[MX] | list[float] - The time list of the previous stimulations (s) pulse_intensity: list[MX] | list[float] The intensity of the stimulations (mA) cn_sum: MX | float @@ -157,11 +162,16 @@ def system_dynamics( 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 of each state dx/dt at the current time t """ + t_stim_prev = self.all_stim + if self.all_stim != self.stim_time: + pulse_intensity = self.previous_stim["pulse_intensity"] + pulse_intensity cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev, pulse_intensity) f_dot = self.f_dot_fun( cn, @@ -171,6 +181,7 @@ def system_dynamics( 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 @@ -234,6 +245,7 @@ def dynamics( 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 @@ -269,15 +281,13 @@ def dynamics( if model.is_approximated: cn_sum = controls[0] - stim_apparition = None intensity_parameters = None else: cn_sum = None intensity_parameters = model.get_intensity_parameters(nlp, parameters) - stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) - if len(intensity_parameters) == 1 and len(stim_apparition) != 1: - intensity_parameters = intensity_parameters * len(stim_apparition) + if len(intensity_parameters) == 1 and len(nlp.model.stim_time) != 1: + intensity_parameters = intensity_parameters * len(nlp.model.stim_time) return DynamicsEvaluation( dxdt=dxdt_fun( @@ -287,11 +297,11 @@ def dynamics( tau1=states[3], km=states[4], t=time, - t_stim_prev=stim_apparition, pulse_intensity=intensity_parameters, cn_sum=cn_sum, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, + passive_force_relationship=passive_force_relationship, ), defects=None, ) diff --git a/cocofest/models/state_configure.py b/cocofest/models/state_configure.py index 80f27b2..a8401c7 100644 --- a/cocofest/models/state_configure.py +++ b/cocofest/models/state_configure.py @@ -253,6 +253,23 @@ def configure_a_calculation(ocp, nlp, muscle_name: str = None): name_cn_sum = [name] return ConfigureProblem.configure_new_variable(name, name_cn_sum, ocp, nlp, as_states=False, as_controls=True) + @staticmethod + def configure_last_pulse_width(ocp, nlp, muscle_name: str = None): + """ + Configure the last pulse width occurred + + Parameters + ---------- + ocp: OptimalControlProgram + A reference to the ocp + nlp: NonLinearProgram + A reference to the phase + """ + muscle_name = "_" + muscle_name if muscle_name else "" + name = "last_pulse_width" + muscle_name + last_pulse_width = [name] + return ConfigureProblem.configure_new_variable(name, last_pulse_width, 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: diff --git a/cocofest/optimization/fes_ocp.py b/cocofest/optimization/fes_ocp.py index 39c4607..d593e97 100644 --- a/cocofest/optimization/fes_ocp.py +++ b/cocofest/optimization/fes_ocp.py @@ -1,4 +1,7 @@ import numpy as np +from math import gcd +from fractions import Fraction +from functools import reduce from bioptim import ( BoundsList, @@ -23,6 +26,7 @@ from ..models.fes_model import FesModel from ..models.dynamical_model import FesMskModel from ..models.ding2003 import DingModelFrequency +from ..models.ding2003_with_fatigue import DingModelFrequencyWithFatigue from ..models.ding2007 import DingModelPulseWidthFrequency from ..models.ding2007_with_fatigue import DingModelPulseWidthFrequencyWithFatigue from ..models.hmed2018 import DingModelPulseIntensityFrequency @@ -39,8 +43,7 @@ class OcpFes: @staticmethod def _prepare_optimization_problem(input_dict: dict) -> dict: - (pulse_event, pulse_width, pulse_intensity, objective) = OcpFes._fill_dict( - input_dict["pulse_event"], + (pulse_width, pulse_intensity, objective) = OcpFes._fill_dict( input_dict["pulse_width"], input_dict["pulse_intensity"], input_dict["objective"], @@ -50,7 +53,6 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: model=input_dict["model"], n_shooting=input_dict["n_shooting"], final_time=input_dict["final_time"], - pulse_event=pulse_event, pulse_width=pulse_width, pulse_intensity=pulse_intensity, objective=objective, @@ -61,29 +63,28 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: (parameters, parameters_bounds, parameters_init, parameter_objectives) = OcpFes._build_parameters( model=input_dict["model"], - stim_time=input_dict["stim_time"], - pulse_event=pulse_event, pulse_width=pulse_width, pulse_intensity=pulse_intensity, use_sx=input_dict["use_sx"], ) OcpFes.update_model_param(input_dict["model"], parameters) - dynamics = OcpFes._declare_dynamics(input_dict["model"]) - x_bounds, x_init = OcpFes._set_bounds(input_dict["model"]) + numerical_data_time_series = OcpFes.set_all_stim_time_for_numerical_data_time_series(input_dict["model"], + input_dict["n_shooting"], + input_dict["final_time"]) - if input_dict["model"].is_approximated: - constraints = OcpFes._build_constraints( - input_dict["model"], - input_dict["n_shooting"], - input_dict["final_time"], - input_dict["stim_time"], - input_dict["control_type"], - ) - u_bounds, u_init = OcpFes._set_u_bounds(input_dict["model"]) - else: - constraints = ConstraintList() - u_bounds, u_init = BoundsList(), InitialGuessList() + dynamics = OcpFes._declare_dynamics(input_dict["model"], numerical_data_time_series) + x_bounds, x_init = OcpFes._set_bounds(input_dict["model"]) + u_bounds, u_init = OcpFes._set_u_bounds(input_dict["model"]) + + constraints = OcpFes._build_constraints( + input_dict["model"], + input_dict["n_shooting"], + input_dict["final_time"], + input_dict["model"].stim_time, + input_dict["control_type"], + pulse_width["bimapping"], + ) objective_functions = OcpFes._set_objective(input_dict["n_shooting"], objective) @@ -113,14 +114,12 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: @staticmethod def prepare_ocp( model: FesModel = None, - stim_time: list = None, final_time: int | float = None, - pulse_event: dict = None, pulse_width: dict = None, pulse_intensity: dict = None, objective: dict = None, use_sx: bool = True, - ode_solver: OdeSolver = OdeSolver.RK4(n_integration_steps=1), + ode_solver: OdeSolver = OdeSolver.RK1(n_integration_steps=10), control_type: ControlType = ControlType.CONSTANT, n_threads: int = 1, ): @@ -131,12 +130,8 @@ def prepare_ocp( ---------- model : FesModel The model type used for the OCP. - stim_time : list - All the stimulation time. final_time : int | float The final time of the OCP. - pulse_event : dict - Dictionary containing parameters related to the appearance of the pulse. pulse_width : dict Dictionary containing parameters related to the duration of the pulse. Optional if not using DingModelPulseWidthFrequency or DingModelPulseWidthFrequencyWithFatigue. @@ -162,10 +157,8 @@ def prepare_ocp( input_dict = { "model": model, - "stim_time": stim_time, - "n_shooting": OcpFes.prepare_n_shooting(stim_time, final_time), + "n_shooting": OcpFes.prepare_n_shooting(model.stim_time, final_time), "final_time": final_time, - "pulse_event": pulse_event, "pulse_width": pulse_width, "pulse_intensity": pulse_intensity, "objective": objective, @@ -208,28 +201,33 @@ def prepare_n_shooting(stim_time, final_time): int The number of shooting points """ - stim_time_str = [str(t) for t in stim_time] - stim_time_str = [ - stim_time_str[i] + ".0" if len(stim_time_str[i]) == 1 else stim_time_str[i] - for i in range(len(stim_time_str)) - ] - nb_decimal = max([len(stim_time_str[i].split(".")[1]) for i in range(len(stim_time))]) - nb_decimal = 2 if nb_decimal < 2 else nb_decimal - decimal_shooting = int("1" + "0" * nb_decimal) - n_shooting = int(final_time * decimal_shooting) + # Represent the final time as a Fraction for exact arithmetic. + T_final = Fraction(final_time).limit_denominator() + n_shooting = 1 + + for t in stim_time: + # Convert the stimulation time to an exact fraction. + t_frac = Fraction(t).limit_denominator() + # Compute the normalized time: t / final_time. + # This fraction is automatically reduced to the lowest terms. + norm = t_frac / T_final + # The denominator in the reduced fraction gives the requirement. + d = norm.denominator + n_shooting = n_shooting * d // gcd(n_shooting, d) + + if n_shooting >= 1000: + print(f"Warning: The number of shooting nodes is very high n = {n_shooting}.\n" + "The optimization might be long, consider using stimulation time with even spacing (common frequency).") + return n_shooting @staticmethod - def _fill_dict(pulse_event, pulse_width, pulse_intensity, objective): + def _fill_dict(pulse_width, pulse_intensity, objective): """ This method fills the provided dictionaries with default values if they are not set. Parameters ---------- - pulse_event : dict - Dictionary containing parameters related to the appearance of the pulse. - Expected keys are 'min', 'max', 'bimapping', 'frequency', 'round_down', and 'pulse_mode'. - pulse_width : dict Dictionary containing parameters related to the duration of the pulse. Expected keys are 'fixed', 'min', 'max', and 'bimapping'. @@ -244,18 +242,9 @@ def _fill_dict(pulse_event, pulse_width, pulse_intensity, objective): Returns ------- - Returns four dictionaries: pulse_event, pulse_width, pulse_intensity, and objective. + Returns four dictionaries: pulse_width, pulse_intensity, and objective. Each dictionary is filled with default values for any keys that were not initially set. """ - pulse_event = {} if pulse_event is None else pulse_event - default_pulse_event = { - "min": None, - "max": None, - "bimapping": False, - "frequency": None, - "round_down": False, - "pulse_mode": "single", - } pulse_width = {} if pulse_width is None else pulse_width default_pulse_width = { @@ -281,19 +270,17 @@ def _fill_dict(pulse_event, pulse_width, pulse_intensity, objective): "custom": None, } - pulse_event = {**default_pulse_event, **pulse_event} pulse_width = {**default_pulse_width, **pulse_width} pulse_intensity = {**default_pulse_intensity, **pulse_intensity} objective = {**default_objective, **objective} - return pulse_event, pulse_width, pulse_intensity, objective + return pulse_width, pulse_intensity, objective @staticmethod def _sanity_check( model=None, n_shooting=None, final_time=None, - pulse_event=None, pulse_width=None, pulse_intensity=None, objective=None, @@ -318,23 +305,6 @@ def _sanity_check( if not isinstance(final_time, int | float) or final_time < 0: raise TypeError("final_time must be a positive int or float type") - if pulse_event["pulse_mode"] != "single": - raise NotImplementedError(f"Pulse mode '{pulse_event['pulse_mode']}' is not yet implemented") - - if pulse_event["frequency"]: - if isinstance(pulse_event["frequency"], int | float): - if pulse_event["frequency"] <= 0: - raise ValueError("frequency must be positive") - else: - raise TypeError("frequency must be int or float type") - - if [pulse_event["min"], pulse_event["max"]].count(None) == 1: - raise ValueError("min and max time event must be both entered or none of them in order to work") - - if pulse_event["bimapping"]: - if not isinstance(pulse_event["bimapping"], bool): - raise TypeError("time bimapping must be bool type") - if isinstance(model, DingModelPulseWidthFrequency | DingModelPulseWidthFrequencyWithFatigue): if pulse_width["fixed"] is None and [pulse_width["min"], pulse_width["max"]].count(None) != 0: raise ValueError("pulse width or pulse width min max bounds need to be set for this model") @@ -483,8 +453,6 @@ def _build_fourier_coefficient(force_tracking): @staticmethod def _build_parameters( model, - stim_time, - pulse_event, pulse_width, pulse_intensity, use_sx, @@ -494,34 +462,7 @@ def _build_parameters( parameters_init = InitialGuessList() parameter_objectives = ParameterObjectiveList() - n_stim = len(stim_time) - parameters.add( - name="pulse_apparition_time", - function=DingModelFrequency.set_pulse_apparition_time, - size=len(stim_time), - scaling=VariableScaling("pulse_apparition_time", [1] * n_stim), - ) - - if pulse_event["min"] and pulse_event["max"]: - time_min_list = np.array([0] + list(np.cumsum([pulse_event["min"]] * (n_stim - 1)))) - time_max_list = np.array([0] + list(np.cumsum([pulse_event["max"]] * (n_stim - 1)))) - parameters_init["pulse_apparition_time"] = np.array( - [(time_max_list[i] + time_min_list[i]) / 2 for i in range(n_stim)] - ) - else: - time_min_list = stim_time - time_max_list = stim_time - parameters_init["pulse_apparition_time"] = np.array(stim_time) - - parameters_bounds.add( - "pulse_apparition_time", - min_bound=time_min_list, - max_bound=time_max_list, - interpolation=InterpolationType.CONSTANT, - ) - - if pulse_event["bimapping"] and pulse_event["min"] and pulse_event["max"]: - raise NotImplementedError("Bimapping is not yet implemented for pulse event") + n_stim = len(model.stim_time) if isinstance(model, DingModelPulseWidthFrequency): if pulse_width["bimapping"]: @@ -621,53 +562,63 @@ def _build_parameters( return (parameters, parameters_bounds, parameters_init, parameter_objectives) @staticmethod - def _build_constraints(model, n_shooting, final_time, stim_time, control_type): + def _build_constraints(model, n_shooting, final_time, stim_time, control_type, bimapped_parameters=False): constraints = ConstraintList() - time_vector = np.linspace(0, final_time, n_shooting + 1) - stim_at_node = [np.where(stim_time[i] <= time_vector)[0][0] for i in range(len(stim_time))] - additional_nodes = 1 if control_type == ControlType.LINEAR_CONTINUOUS else 0 - if model._sum_stim_truncation: - max_stim_to_keep = model._sum_stim_truncation - else: - max_stim_to_keep = 10000000 - - index_sup = 0 - index_inf = 0 - - for i in range(n_shooting + additional_nodes): - if i in stim_at_node: - index_sup += 1 - if index_sup >= max_stim_to_keep: - index_inf = index_sup - max_stim_to_keep - - constraints.add( - CustomConstraint.cn_sum, - node=i, - stim_time=stim_time[index_inf:index_sup], - ) - if isinstance(model, DingModelPulseWidthFrequency): - index = 0 + for i in range(n_shooting): + constraints.add( + CustomConstraint.last_pulse_width, + last_stim_index=i if not bimapped_parameters else 0, + node=i, + ) + + if model.is_approximated: + time_vector = np.linspace(0, final_time, n_shooting + 1) + stim_at_node = [np.where(stim_time[i] <= time_vector)[0][0] for i in range(len(stim_time))] + additional_nodes = 1 if control_type == ControlType.LINEAR_CONTINUOUS else 0 + if model._sum_stim_truncation: + max_stim_to_keep = model._sum_stim_truncation + else: + max_stim_to_keep = 10000000 + + index_sup = 0 + index_inf = 0 + for i in range(n_shooting + additional_nodes): - if i in stim_at_node and i != 0: - index += 1 + if i in stim_at_node: + index_sup += 1 + if index_sup >= max_stim_to_keep: + index_inf = index_sup - max_stim_to_keep + constraints.add( - CustomConstraint.a_calculation, + CustomConstraint.cn_sum, node=i, - last_stim_index=index, + stim_time=stim_time[index_inf:index_sup], ) + if isinstance(model, DingModelPulseWidthFrequency): + index = 0 + for i in range(n_shooting + additional_nodes): + if i in stim_at_node and i != 0: + index += 1 + constraints.add( + CustomConstraint.a_calculation, + node=i, + last_stim_index=index, + ) + return constraints @staticmethod - def _declare_dynamics(model): + def _declare_dynamics(model, numerical_data_timeseries=None): 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, ) return dynamics @@ -694,7 +645,9 @@ def _set_bounds(model): ) for i in range(len(variable_bound_list)): - if variable_bound_list[i] == "Cn" or variable_bound_list[i] == "F": + if variable_bound_list[i] == "Cn": + max_bounds[i] = 2 + if variable_bound_list[i] == "F": max_bounds[i] = 1000 elif variable_bound_list[i] == "Tau1" or variable_bound_list[i] == "Km": max_bounds[i] = 1 @@ -722,13 +675,13 @@ def _set_bounds(model): def _set_u_bounds(model): # Controls bounds u_bounds = BoundsList() - # Controls initial guess u_init = InitialGuessList() - u_init.add(key="Cn_sum", initial_guess=[0], phase=0) - if isinstance(model, DingModelPulseWidthFrequency): - u_init.add(key="A_calculation", initial_guess=[0], phase=0) - + u_init.add(key="last_pulse_width", initial_guess=[0], phase=0) + if model.is_approximated: + u_init.add(key="Cn_sum", initial_guess=[0], phase=0) + if isinstance(model, DingModelPulseWidthFrequency): + u_init.add(key="A_calculation", initial_guess=[0], phase=0) return u_bounds, u_init @staticmethod @@ -781,3 +734,38 @@ def update_model_param(model, parameters): param_scaling = parameters[param_key].scaling.scaling param_reduced = parameters[param_key].cx parameters[param_key].function(model, param_reduced * param_scaling, **parameters[param_key].kwargs) + + @staticmethod + def set_all_stim_time_for_numerical_data_time_series(model, n_shooting, final_time): + truncation = model._sum_stim_truncation + # --- Set the previous stim time for the numerical data time series (mandatory to avoid nan values) --- # + while len(model.previous_stim["time"]) < truncation: + model.previous_stim["time"].insert(0, -10000000) + if isinstance(model, DingModelPulseWidthFrequency): + model.previous_stim["pulse_width"].insert(0, 0.0003) + if isinstance(model, DingModelPulseIntensityFrequency): + model.previous_stim["pulse_intensity"].insert(0, 50) + + model.all_stim = model.previous_stim["time"] + model.stim_time + stim_time = np.array(model.all_stim) + dt = final_time / n_shooting + + # For each node (n_shooting+1 total), find the last index where stim_time <= node_time. + node_idx = [np.where(stim_time <= i * dt)[0][-1] for i in range(n_shooting + 1)] + + # For each node, extract the stim times up to that node, then keep only the last 'truncated' values. + stim_time_list = [ + list(stim_time[:idx + 1][-truncation:]) + for idx in node_idx + ] + + # --- Create a correct numerical_data_time_series shape array from the stim_time_list --- # + reshaped_array = np.full((n_shooting+1, truncation), np.nan) + for i in range(len(stim_time_list)): + reshaped_array[i] = np.array(stim_time_list[i]) + + # --- Reshape the array to obtain the desired final shape --- # + temp_result = reshaped_array[:, np.newaxis, :] + stim_time_array = np.transpose(temp_result, (2, 1, 0)) + + return {"stim_time": stim_time_array} diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 6793b36..8218de5 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -1,12 +1,12 @@ import numpy as np from bioptim import ( - Axis, BoundsList, ConstraintList, ControlType, ConstraintFcn, DynamicsList, + ExternalForceSetTimeSeries, InitialGuessList, InterpolationType, Node, @@ -22,9 +22,6 @@ ) from ..custom_objectives import CustomObjective -from ..dynamics.inverse_kinematics_and_dynamics import get_circle_coord -from ..dynamics.initial_guess_warm_start import get_initial_guess -from ..models.ding2003 import DingModelFrequency from ..models.ding2007 import DingModelPulseWidthFrequency from ..models.dynamical_model import FesMskModel from ..models.hmed2018 import DingModelPulseIntensityFrequency @@ -37,8 +34,7 @@ class OcpFesMsk: @staticmethod def _prepare_optimization_problem(input_dict: dict) -> dict: - (pulse_event, pulse_width, pulse_intensity, objective) = OcpFes._fill_dict( - input_dict["pulse_event"], + (pulse_width, pulse_intensity, objective) = OcpFes._fill_dict( input_dict["pulse_width"], input_dict["pulse_intensity"], input_dict["objective"], @@ -55,7 +51,6 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: model=input_dict["model"], n_shooting=input_dict["n_shooting"], final_time=input_dict["final_time"], - pulse_event=pulse_event, pulse_width=pulse_width, pulse_intensity=pulse_intensity, objective=objective, @@ -77,8 +72,6 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: parameter_objectives, ) = OcpFesMsk._build_parameters( model=input_dict["model"], - stim_time=input_dict["stim_time"], - pulse_event=pulse_event, pulse_width=pulse_width, pulse_intensity=pulse_intensity, use_sx=input_dict["use_sx"], @@ -88,36 +81,31 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: input_dict["model"], input_dict["n_shooting"], input_dict["final_time"], - input_dict["stim_time"], input_dict["control_type"], msk_info["custom_constraint"], + input_dict["external_forces"], + input_dict["n_cycles_simultaneous"] if "n_cycles_simultaneous" in input_dict.keys() else 1, ) - dynamics = OcpFesMsk._declare_dynamics(input_dict["model"]) - initial_state = ( - get_initial_guess( - input_dict["model"].path, - input_dict["final_time"], - input_dict["n_shooting"], - objective, - n_threads=input_dict["n_threads"], - ) - if input_dict["initial_guess_warm_start"] - else None - ) + if input_dict["external_forces"]: + input_dict["n_total_cycles"] = input_dict["n_total_cycles"] if "n_total_cycles" in input_dict.keys() else 1 + numerical_time_series, with_contact, external_force_set = OcpFesMsk._prepare_numerical_time_series(input_dict) + else: + numerical_time_series, with_contact, external_force_set = None, False, None - x_bounds, x_init = OcpFesMsk._set_bounds( - input_dict["model"], - msk_info, - initial_state, - input_dict["n_cycles_simultaneous"] if "n_cycles_simultaneous" in input_dict.keys() else 1, - ) - u_bounds, u_init = OcpFesMsk._set_u_bounds(input_dict["model"], msk_info["with_residual_torque"]) + dynamics = OcpFesMsk._declare_dynamics(input_dict["model"], numerical_time_series=numerical_time_series, with_contact=with_contact) + + x_bounds, x_init = OcpFesMsk._set_bounds_fes(input_dict["model"]) + x_bounds, x_init = OcpFesMsk._set_bounds_msk(x_bounds, x_init, input_dict["model"], msk_info) + + u_bounds, u_init = OcpFesMsk._set_u_bounds_fes(input_dict["model"]) + u_bounds, u_init = OcpFesMsk._set_u_bounds_msk(u_bounds, u_init, input_dict["model"], msk_info["with_residual_torque"]) muscle_force_key = [ "F_" + input_dict["model"].muscles_dynamics_model[i].muscle_name for i in range(len(input_dict["model"].muscles_dynamics_model)) ] + objective_functions = OcpFesMsk._set_objective( input_dict["n_shooting"], muscle_force_key, @@ -130,10 +118,13 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: name=input_dict["model"].name, biorbd_path=input_dict["model"].biorbd_path, muscles_model=input_dict["model"].muscles_dynamics_model, + stim_time=input_dict["model"].muscles_dynamics_model[0].stim_time, + previous_stim=input_dict["model"].muscles_dynamics_model[0].previous_stim, activate_force_length_relationship=input_dict["model"].activate_force_length_relationship, activate_force_velocity_relationship=input_dict["model"].activate_force_velocity_relationship, activate_residual_torque=input_dict["model"].activate_residual_torque, parameters=parameters, + external_force_set=external_force_set, ) optimization_dict = { @@ -162,9 +153,7 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: @staticmethod def prepare_ocp( model: FesMskModel = None, - stim_time: list = None, final_time: int | float = None, - pulse_event: dict = None, pulse_width: dict = None, pulse_intensity: dict = None, objective: dict = None, @@ -174,6 +163,7 @@ def prepare_ocp( ode_solver: OdeSolverBase = OdeSolver.RK4(n_integration_steps=1), control_type: ControlType = ControlType.CONSTANT, n_threads: int = 1, + external_forces: dict = None, ): """ Prepares the Optimal Control Program (OCP) with a musculoskeletal model for a movement to be solved. @@ -182,12 +172,8 @@ def prepare_ocp( ---------- model : FesModel The FES model to use. - stim_time : list - The stimulation times. final_time : int | float The final time of the OCP. - pulse_event : dict - Dictionary containing parameters related to the appearance of the pulse. It should contain the following keys: "min", "max", "bimapping", "frequency", "round_down", "pulse_mode". pulse_width : dict Dictionary containing parameters related to the duration of the pulse. @@ -211,6 +197,8 @@ def prepare_ocp( The type of control to use. n_threads : int The number of threads to use while solving (multi-threading if > 1). + external_forces : dict + Dictionary containing the parameters related to the external forces. Returns ------- @@ -220,10 +208,8 @@ def prepare_ocp( input_dict = { "model": model, - "stim_time": stim_time, - "n_shooting": OcpFes.prepare_n_shooting(stim_time, final_time), + "n_shooting": OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, final_time), "final_time": final_time, - "pulse_event": pulse_event, "pulse_width": pulse_width, "pulse_intensity": pulse_intensity, "objective": objective, @@ -233,6 +219,7 @@ def prepare_ocp( "ode_solver": ode_solver, "n_threads": n_threads, "control_type": control_type, + "external_forces": external_forces, } optimization_dict = OcpFesMsk._prepare_optimization_problem(input_dict) @@ -283,7 +270,6 @@ def _fill_msk_dict(pulse_width, pulse_intensity, objective, msk_info): default_objective = { "force_tracking": None, "end_node_tracking": None, - "cycling_objective": None, "custom": None, "q_tracking": None, "minimize_muscle_fatigue": False, @@ -307,7 +293,24 @@ def _fill_msk_dict(pulse_width, pulse_intensity, objective, msk_info): return pulse_width, pulse_intensity, objective, msk_info @staticmethod - def _declare_dynamics(bio_models): + def _prepare_numerical_time_series(input_dict): + + total_n_shooting = input_dict["n_shooting"] * input_dict["n_cycles_simultaneous"] + total_external_forces_frame = input_dict["n_total_cycles"] * input_dict["n_shooting"] if input_dict["n_total_cycles"] >= input_dict["n_cycles_simultaneous"] else total_n_shooting + external_force_set = ExternalForceSetTimeSeries(nb_frames=total_external_forces_frame) + + external_force_array = np.array(input_dict["external_forces"]["torque"]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, total_external_forces_frame)) + external_force_set.add_torque(segment=input_dict["external_forces"]["Segment_application"], + values=reshape_values_array) + + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + with_contact = input_dict["external_forces"]["with_contact"] if "with_contact" in input_dict["external_forces"].keys() else False + + return numerical_time_series, with_contact, external_force_set + + @staticmethod + def _declare_dynamics(bio_models, numerical_time_series, with_contact): dynamics = DynamicsList() dynamics.add( bio_models.declare_model_variables, @@ -316,14 +319,14 @@ def _declare_dynamics(bio_models): expand_continuity=False, phase=0, phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + numerical_data_timeseries=numerical_time_series, + with_contact=with_contact, ) return dynamics @staticmethod def _build_parameters( model: FesMskModel, - stim_time: list, - pulse_event: dict, pulse_width: dict, pulse_intensity: dict, use_sx: bool = True, @@ -333,22 +336,7 @@ def _build_parameters( parameters_init = InitialGuessList() parameter_objectives = ParameterObjectiveList() - n_stim = len(stim_time) - parameters.add( - name="pulse_apparition_time", - function=DingModelFrequency.set_pulse_apparition_time, - size=n_stim, - scaling=VariableScaling("pulse_apparition_time", [1] * n_stim), - ) - - parameters_bounds.add( - "pulse_apparition_time", - min_bound=np.array(stim_time), - max_bound=np.array(stim_time), - interpolation=InterpolationType.CONSTANT, - ) - - parameters_init["pulse_apparition_time"] = np.array(stim_time) + n_stim = len(model.muscles_dynamics_model[0].stim_time) for i in range(len(model.muscles_dynamics_model)): if isinstance(model.muscles_dynamics_model[i], DingModelPulseWidthFrequency): @@ -473,19 +461,21 @@ def _build_parameters( ) @staticmethod - def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None): + def _build_constraints(model, n_shooting, final_time, control_type, custom_constraint=None, external_forces=None, simultaneous_cycle=1): constraints = ConstraintList() + stim_time = model.muscles_dynamics_model[0].stim_time - if model.activate_residual_torque: + if model.activate_residual_torque and control_type == ControlType.LINEAR_CONTINUOUS: + applied_node = n_shooting - 1 if external_forces else Node.END constraints.add( ConstraintFcn.TRACK_CONTROL, - node=Node.END, + node=applied_node, key="tau", target=np.zeros(model.nb_tau), ) if model.muscles_dynamics_model[0].is_approximated: - time_vector = np.linspace(0, final_time, n_shooting + 1) + time_vector = np.linspace(0, final_time*simultaneous_cycle, n_shooting*simultaneous_cycle + 1) stim_at_node = [np.where(stim_time[i] <= time_vector)[0][0] for i in range(len(stim_time))] additional_node = 1 if control_type == ControlType.LINEAR_CONTINUOUS else 0 @@ -529,7 +519,7 @@ def _build_constraints(model, n_shooting, final_time, stim_time, control_type, c return constraints @staticmethod - def _set_bounds(bio_models, msk_info, initial_state, n_cycles_simultaneous=1): + def _set_bounds_fes(bio_models): # ---- STATE BOUNDS REPRESENTATION ---- # # # |‾‾‾‾‾‾‾‾‾‾x_max_middle‾‾‾‾‾‾‾‾‾‾‾‾x_max_end‾ @@ -579,6 +569,10 @@ def _set_bounds(bio_models, msk_info, initial_state, n_cycles_simultaneous=1): for j in range(len(variable_bound_list)): x_init.add(variable_bound_list[j], model.standard_rest_values()[j], phase=0) + return x_bounds, x_init + + @staticmethod + def _set_bounds_msk(x_bounds, x_init, bio_models, msk_info): if msk_info["bound_type"] == "start_end": start_bounds = [] end_bounds = [] @@ -618,52 +612,13 @@ def _set_bounds(bio_models, msk_info, initial_state, n_cycles_simultaneous=1): x_bounds.add(key="q", bounds=q_x_bounds, phase=0) x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) - # Sets the initial state of q, qdot and muscle forces for all the phases if a warm start is used - if initial_state: - for key in initial_state.keys(): - repeated_array = np.tile(initial_state[key][:, :-1], (1, n_cycles_simultaneous)) - # Append the last column of the original array to reach the desired shape - result_array = np.hstack((repeated_array, initial_state[key][:, -1:])) - initial_state[key] = result_array - - muscle_names = bio_models.muscle_names - x_init.add( - key="q", - initial_guess=initial_state["q"], - interpolation=InterpolationType.EACH_FRAME, - phase=0, - ) - x_init.add( - key="qdot", - initial_guess=initial_state["qdot"], - interpolation=InterpolationType.EACH_FRAME, - phase=0, - ) - for j in range(len(muscle_names)): - x_init.add( - key="F_" + muscle_names[j], - initial_guess=initial_state[muscle_names[j]], - interpolation=InterpolationType.EACH_FRAME, - phase=0, - ) - else: - x_init.add(key="q", initial_guess=[0] * bio_models.nb_q, phase=0) - return x_bounds, x_init @staticmethod - def _set_u_bounds(bio_models, with_residual_torque): + def _set_u_bounds_fes(bio_models): u_bounds = BoundsList() # Controls bounds u_init = InitialGuessList() # Controls initial guess - if with_residual_torque: # TODO : ADD SEVERAL INDIVIDUAL FIXED RESIDUAL TORQUE FOR EACH JOINT - nb_tau = bio_models.nb_tau - tau_min, tau_max, tau_init = [-50] * nb_tau, [50] * nb_tau, [0] * nb_tau - u_bounds.add( - key="tau", min_bound=tau_min, max_bound=tau_max, phase=0, interpolation=InterpolationType.CONSTANT - ) - u_init.add(key="tau", initial_guess=tau_init, phase=0) - if bio_models.muscles_dynamics_model[0].is_approximated: for i in range(len(bio_models.muscles_dynamics_model)): u_init.add(key="Cn_sum_" + bio_models.muscles_dynamics_model[i].muscle_name, initial_guess=[0], phase=0) @@ -678,6 +633,17 @@ def _set_u_bounds(bio_models, with_residual_torque): return u_bounds, u_init + @staticmethod + def _set_u_bounds_msk(u_bounds, u_init, bio_models, with_residual_torque): + if with_residual_torque: # TODO : ADD SEVERAL INDIVIDUAL FIXED RESIDUAL TORQUE FOR EACH JOINT + nb_tau = bio_models.nb_tau + tau_min, tau_max, tau_init = [-200] * nb_tau, [200] * nb_tau, [0] * nb_tau + u_bounds.add( + key="tau", min_bound=tau_min, max_bound=tau_max, phase=0, interpolation=InterpolationType.CONSTANT + ) + u_init.add(key="tau", initial_guess=tau_init, phase=0) + return u_bounds, u_init + @staticmethod def _set_objective( n_shooting, @@ -736,29 +702,6 @@ def _set_objective( phase=0, ) - if objective["cycling"]: - x_center = objective["cycling"]["x_center"] - y_center = objective["cycling"]["y_center"] - radius = objective["cycling"]["radius"] - circle_coord_list = np.array( - [ - get_circle_coord(theta, x_center, y_center, radius)[:-1] - for theta in np.linspace( - 0, -2 * np.pi * n_simultaneous_cycle, n_shooting * n_simultaneous_cycle + 1 - ) - ] - ) - objective_functions.add( - ObjectiveFcn.Mayer.TRACK_MARKERS, - weight=10000000, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list.T, - node=Node.ALL, - phase=0, - quadratic=True, - ) - if objective["q_tracking"]: q_fourier_coef = [] for i in range(len(objective["q_tracking"][1])): @@ -885,30 +828,6 @@ def _sanity_check_msk_inputs( f"end_node_tracking index {i}: {objective['end_node_tracking'][i]} must be int or float type" ) - if objective["cycling"]: - if not isinstance(objective["cycling"], dict): - raise TypeError(f"cycling_objective: {objective['cycling']} must be dictionary type") - - cycling_objective_keys = ["x_center", "y_center", "radius", "target"] - if not all([cycling_objective_keys[i] in objective["cycling"] for i in range(len(cycling_objective_keys))]): - raise ValueError( - f"cycling_objective dictionary must contain the following keys: {cycling_objective_keys}" - ) - - if not all([isinstance(objective["cycling"][key], int | float) for key in cycling_objective_keys[:3]]): - raise TypeError(f"cycling_objective x_center, y_center and radius inputs must be int or float") - - if isinstance(objective["cycling"][cycling_objective_keys[-1]], str): - if ( - objective["cycling"][cycling_objective_keys[-1]] != "marker" - and objective["cycling"][cycling_objective_keys[-1]] != "q" - ): - raise ValueError( - f"{objective['cycling'][cycling_objective_keys[-1]]} not implemented chose between 'marker' and 'q' as 'target'" - ) - else: - raise TypeError(f"cycling_objective target must be string type") - if objective["q_tracking"]: if not isinstance(objective["q_tracking"], list) and len(objective["q_tracking"]) != 2: raise TypeError("q_tracking should be a list of size 2") diff --git a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py index e091b02..85e4945 100644 --- a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py +++ b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py @@ -4,41 +4,55 @@ OdeSolver, MultiCyclicNonlinearModelPredictiveControl, ControlType, + SolutionMerge, + Solution, ) - +from .fes_ocp import OcpFes from .fes_ocp_dynamics import OcpFesMsk from ..models.dynamical_model import FesMskModel +from ..models.ding2007_with_fatigue import DingModelPulseWidthFrequencyWithFatigue class NmpcFesMsk(MultiCyclicNonlinearModelPredictiveControl): - def advance_window_bounds_states(self, sol, **extra): + def advance_window_bounds_states(self, sol, n_cycles_simultaneous=None, **extra): super(NmpcFesMsk, self).advance_window_bounds_states(sol) self.update_stim(sol) + if self.nlp[0].model.for_cycling: + self.nlp[0].x_bounds["q"].min[-1, :] = self.nlp[0].model.bounds_from_ranges("q").min[-1] * n_cycles_simultaneous + self.nlp[0].x_bounds["q"].max[-1, :] = self.nlp[0].model.bounds_from_ranges("q").max[-1] * n_cycles_simultaneous + return True - def update_stim(self, sol): - stimulation_time = sol.decision_parameters()["pulse_apparition_time"] - stim_prev = list(np.round(np.array(stimulation_time) - sol.ocp.phase_time[0], 3)) + def advance_window_initial_guess_states(self, sol, n_cycles_simultaneous=None): + # Reimplementation of the advance_window method so the rotation of the wheel restart at -pi + super(NmpcFesMsk, self).advance_window_initial_guess_states(sol) + # if cycling else pass + if self.nlp[0].model.for_cycling: + q = sol.decision_states(to_merge=SolutionMerge.NODES)["q"] + self.nlp[0].x_init["q"].init[-1, :] = q[-1, :] # Keep the previously found value for the wheel + return True - for model in self.nlp[0].model.muscles_dynamics_model: - self.nlp[0].model.muscles_dynamics_model[0].stim_prev = stim_prev - if "pulse_intensity_" + model.muscle_name in sol.parameters.keys(): - self.nlp[0].model.muscles_dynamics_model[0].stim_pulse_intensity_prev = list( - sol.parameters["pulse_intensity_" + model.muscle_name] - ) + def update_stim(self, sol): + # only keep the last 10 stimulation times + previous_stim_time = [round(x - self.phase_time[0], 2) for x in self.nlp[0].model.muscles_dynamics_model[0].stim_time[-10:]] # TODO fix this (keep the middle window) + for i in range(len(self.nlp[0].model.muscles_dynamics_model)): + self.nlp[0].model.muscles_dynamics_model[i].previous_stim = {} if self.nlp[0].model.muscles_dynamics_model[i].previous_stim is None else self.nlp[0].model.muscles_dynamics_model[i].previous_stim + self.nlp[0].model.muscles_dynamics_model[i].previous_stim["time"] = previous_stim_time + if isinstance(self.nlp[0].model.muscles_dynamics_model[i], DingModelPulseWidthFrequencyWithFatigue): + self.nlp[0].model.muscles_dynamics_model[i].previous_stim["pulse_width"] = list(sol.parameters["pulse_width_" + self.nlp[0].model.muscles_dynamics_model[i].muscle_name][-10:]) + self.nlp[0].model.muscles_dynamics_model[i].all_stim = self.nlp[0].model.muscles_dynamics_model[i].previous_stim["time"] + self.nlp[0].model.muscles_dynamics_model[i].stim_time @staticmethod def prepare_nmpc( model: FesMskModel = None, - stim_time: list = None, - cycle_len: int = None, cycle_duration: int | float = None, n_cycles_simultaneous: int = None, n_cycles_to_advance: int = None, - pulse_event: dict = None, + n_total_cycles: int = None, pulse_width: dict = None, pulse_intensity: dict = None, objective: dict = None, msk_info: dict = None, + external_forces: dict = None, initial_guess_warm_start: bool = False, use_sx: bool = True, ode_solver: OdeSolver = OdeSolver.RK4(n_integration_steps=1), @@ -48,16 +62,16 @@ def prepare_nmpc( input_dict = { "model": model, - "stim_time": stim_time, - "n_shooting": cycle_len, + "n_shooting": OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration), "final_time": cycle_duration, "n_cycles_simultaneous": n_cycles_simultaneous, "n_cycles_to_advance": n_cycles_to_advance, - "pulse_event": pulse_event, + "n_total_cycles": n_total_cycles, "pulse_width": pulse_width, "pulse_intensity": pulse_intensity, "objective": objective, "msk_info": msk_info, + "external_forces": external_forces, "initial_guess_warm_start": initial_guess_warm_start, "use_sx": use_sx, "ode_solver": ode_solver, @@ -70,7 +84,7 @@ def prepare_nmpc( return NmpcFesMsk( bio_model=[optimization_dict["model"]], dynamics=optimization_dict["dynamics"], - cycle_len=cycle_len, + cycle_len=optimization_dict["n_shooting"], cycle_duration=cycle_duration, n_cycles_simultaneous=n_cycles_simultaneous, n_cycles_to_advance=n_cycles_to_advance, @@ -87,3 +101,67 @@ def prepare_nmpc( n_threads=optimization_dict["n_threads"], control_type=optimization_dict["control_type"], ) + + @staticmethod + def prepare_nmpc_for_cycling( + model: FesMskModel = None, + cycle_duration: int | float = None, + n_cycles_simultaneous: int = None, + n_cycles_to_advance: int = None, + n_total_cycles: int = None, + pulse_width: dict = None, + pulse_intensity: dict = None, + objective: dict = None, + msk_info: dict = None, + external_forces: dict = None, + initial_guess_warm_start: bool = False, + use_sx: bool = True, + ode_solver: OdeSolver = OdeSolver.RK4(n_integration_steps=1), + control_type: ControlType = ControlType.CONSTANT, + n_threads: int = 1, + ): + input_dict = { + "model": model, + "n_shooting": OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration*n_cycles_simultaneous), + "final_time": cycle_duration, + "n_cycles_simultaneous": n_cycles_simultaneous, + "n_cycles_to_advance": n_cycles_to_advance, + "n_total_cycles": n_total_cycles, + "pulse_width": pulse_width, + "pulse_intensity": pulse_intensity, + "objective": objective, + "msk_info": msk_info, + "initial_guess_warm_start": initial_guess_warm_start, + "use_sx": use_sx, + "ode_solver": ode_solver, + "n_threads": n_threads, + "control_type": control_type, + "external_forces": external_forces, + } + + optimization_dict = OcpFesMsk._prepare_optimization_problem(input_dict) + optimization_dict_for_cycling = OcpFesMsk._prepare_optimization_problem_for_cycling(optimization_dict, + input_dict) + + return NmpcFesMsk( + bio_model=[optimization_dict["model"]], + dynamics=optimization_dict["dynamics"], + cycle_len=optimization_dict["n_shooting"], + cycle_duration=cycle_duration, + n_cycles_simultaneous=n_cycles_simultaneous, + n_cycles_to_advance=n_cycles_to_advance, + common_objective_functions=optimization_dict["objective_functions"], + x_init=optimization_dict_for_cycling["x_init"], + x_bounds=optimization_dict_for_cycling["x_bounds"], + u_init=optimization_dict_for_cycling["u_init"], + u_bounds=optimization_dict_for_cycling["u_bounds"], + constraints=optimization_dict_for_cycling["constraints"], + parameters=optimization_dict["parameters"], + parameter_bounds=optimization_dict["parameters_bounds"], + parameter_init=optimization_dict["parameters_init"], + parameter_objectives=optimization_dict["parameter_objectives"], + control_type=control_type, + use_sx=optimization_dict["use_sx"], + ode_solver=optimization_dict["ode_solver"], + n_threads=optimization_dict["n_threads"], + ) diff --git a/cocofest/result/graphics.py b/cocofest/result/graphics.py new file mode 100644 index 0000000..7adabea --- /dev/null +++ b/cocofest/result/graphics.py @@ -0,0 +1,369 @@ +from bioptim import Solution, SolutionMerge, InterpolationType +import matplotlib.pyplot as plt +import numpy as np +from ..models.ding2007 import DingModelPulseWidthFrequency +from ..models.dynamical_model import FesMskModel + + +class FES_plot: + def __init__(self, data: Solution | dict): + self.data = data + + def plot(self, title: str = None, show_stim: bool = False, show_bounds: bool = False): + if isinstance(self.data, Solution): + if isinstance(self.data.ocp.nlp[0].model, FesMskModel): + self.msk_plot(title, show_stim, show_bounds) + else: + self.ocp_plot(title, show_stim, show_bounds) + + elif isinstance(self.data, dict): + if show_stim or show_bounds: + raise ValueError("Cannot show stim or bounds with data dictionary type") + self.ivp_plot(title) + else: + raise ValueError("Data must be a Solution or a dictionary") + + def get_data(self, solution: Solution): + states = solution.stepwise_states(to_merge=SolutionMerge.NODES) + controls = solution.stepwise_controls(to_merge=SolutionMerge.NODES) + time = solution.stepwise_time(to_merge=SolutionMerge.NODES).T[0] + force = states[self.force_keys[0]] + for i in range(1, len(self.force_keys)): + force = np.append(force, states[self.force_keys[i]], axis=0) + q = states["q"] if "q" in states else None + qdot = states["qdot"] if "qdot" in states else None + tau = controls["tau"] if "tau" in controls else None + return q, qdot, tau, force, time + + def get_msk_bounds(self, solution: Solution): + q_bounds = [solution.ocp.nlp[0].x_bounds["q"].min[0], solution.ocp.nlp[0].x_bounds["q"].max[0]] + qdot_bounds = [solution.ocp.nlp[0].x_bounds["qdot"].min[0], solution.ocp.nlp[0].x_bounds["qdot"].max[0]] + tau_bounds = [solution.ocp.nlp[0].u_bounds["tau"].min[0], solution.ocp.nlp[0].u_bounds["tau"].max[0]] if "tau" in solution.ocp.nlp[0].u_bounds else None + force_bounds = {} + for i in range(len(self.force_keys)): + force_bounds[self.force_keys[i]] = [solution.ocp.nlp[0].x_bounds[self.force_keys[i]].min[0], solution.ocp.nlp[0].x_bounds[self.force_keys[i]].max[0]] + bounds = {"q": q_bounds, "qdot": qdot_bounds, "tau": tau_bounds, "force": force_bounds} + return bounds + + def get_bounds(self, solution: Solution): + if solution.ocp.nlp[0].x_bounds.type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: + solution_bounds = solution.ocp.nlp[0].x_bounds + cn_bounds = [list(solution_bounds["Cn"].min[0]), list(solution_bounds["Cn"].max[0])] + force_bounds = [list(solution_bounds["F"].min[0]), list(solution_bounds["F"].max[0])] + bounds = {"cn": cn_bounds, "force": force_bounds} + if solution.ocp.nlp[0].model._with_fatigue: + a_bounds = [list(solution_bounds["A"].min[0]), list(solution_bounds["A"].max[0])] + tau1_bounds = [list(solution_bounds["Tau1"].min[0]), list(solution_bounds["Tau1"].max[0])] + km_bounds = [list(solution_bounds["Km"].min[0]), list(solution_bounds["Km"].max[0])] + bounds = {"cn": cn_bounds, "force": force_bounds, "a": a_bounds, "tau1": tau1_bounds, "km": km_bounds} + + temp_time = solution.decision_time(to_merge=SolutionMerge.NODES).T[0] + bound_time = [temp_time[0], temp_time[1], temp_time[-1]] + + elif solution.ocp.nlp[0].x_bounds.type == InterpolationType.EACH_FRAME: + solution_bounds = solution.ocp.nlp[0].x_bounds + cn_bounds = [list(solution_bounds["Cn"].min[0]), list(solution_bounds["Cn"].max[0])] + force_bounds = [list(solution_bounds["F"].min[0]), list(solution_bounds["F"].max[0])] + bounds = {"cn": cn_bounds, "force": force_bounds} + + if solution.ocp.nlp[0].model._with_fatigue: + a_bounds = [list(solution_bounds["A"].min[0]), list(solution_bounds["A"].max[0])] + tau1_bounds = [list(solution_bounds["Tau1"].min[0]), list(solution_bounds["Tau1"].max[0])] + km_bounds = [list(solution_bounds["Km"].min[0]), list(solution_bounds["Km"].max[0])] + bounds = {"cn": cn_bounds, "force": force_bounds, "a": a_bounds, "tau1": tau1_bounds, "km": km_bounds} + + bound_time = solution.decision_time(to_merge=SolutionMerge.NODES).T[0] + + else: + raise NotImplementedError("Bounds type not implemented") + + return bounds, bound_time + + def axes_settings(self, axes_list): + for i in range(len(axes_list)): + offset = -0.2 * i if len(axes_list) > 1 else -0.05 + axes_list[i].spines["left"].set_position(("axes", offset)) + axes_list[i].set_frame_on(True) + for spine in axes_list[i].spines.values(): + spine.set_visible(False) + axes_list[i].spines["left"].set_visible(True) + + def build_several_y_axis(self, axis, time, values, labels: list = None): + n = values.shape[0] + cmap = plt.get_cmap("tab20", n) + colors = [cmap(i) for i in range(n)] + axes_list = [axis] + [axis.twinx() for _ in range(values.shape[0] - 1)] + + lines_list, labels_list = [], [] + for i in range(len(axes_list)): + axes_list[i].spines["left"].set_color(colors[i]) + axes_list[i].yaxis.set_label_position('left') + axes_list[i].yaxis.set_ticks_position('left') + axes_list[i].plot(time, values[i], color=colors[i], label=labels[i], lw=3) + axes_list[i].set_ylabel(labels[i]) + lines, fig_labels = axes_list[i].get_legend_handles_labels() + lines_list.append(lines) + labels_list.append(fig_labels) + + self.axes_settings(axes_list) + axis.legend(lines_list, labels_list) + axis.set_xlabel("Time [s]") + + def build_several_y_axis_FES(self, axis, time, values, labels: list = None, stim_time=None, stim_values=None, axes_title=None): + n = len(labels) + cmap = plt.get_cmap("tab20b", n) + colors = [cmap(i) for i in range(n)] + axes_list = [axis, axis.twinx()] + axes_colors = ["red", "green"] + stim_values_keys = list(stim_values.keys()) + + lines_list, labels_list = [], [] + for i in range(len(axes_list)): + axes_list[i].spines["left"].set_color(axes_colors[i]) + axes_list[i].yaxis.set_label_position('left') + axes_list[i].yaxis.set_ticks_position('left') + axes_list[i].set_ylabel(axes_title[i]) + lines, fig_labels = axes_list[i].get_legend_handles_labels() + lines_list.append(lines) + labels_list.append(fig_labels) + if i == 0: + [axes_list[i].plot(time, values[j], color=colors[j], label=labels[j], lw=3) for j in range(n)] + if i == 1: + stim_offset = ([(i - n // 2) * 0.01 for i in range(n)] + if n % 2 != 0 + else [-(n / 2) * 0.01 + 0.01 * i for i in range(n)]) + for j in range(n): + [axes_list[i].axvline(x=stim_time[k] + stim_offset[j], ymin=0, ymax=stim_values[stim_values_keys[j]][k], color=colors[j], linestyle="--", lw=1) for k in range(len(stim_time))] + + self.axes_settings(axes_list) + axis.legend(lines_list, labels_list) + axis.set_xlabel("Time [s]") + + def create_twin_axes(self, ax, time, data, label, color, lw, ylabel, tick_color): + """Create a twin y-axis on ax with custom styling.""" + twin_ax = ax.twinx() + twin_ax.spines["left"].set_color(tick_color) + twin_ax.yaxis.set_label_position('left') + twin_ax.yaxis.set_ticks_position('left') + line, = twin_ax.plot(time, data, color=color, label=label, lw=lw) + twin_ax.set_ylabel(ylabel) + self.axes_settings([twin_ax]) + return twin_ax, line + + def ocp_plot(self, title: str = None, show_stim: bool = True, show_bounds: bool = True): + solution = self.data + states = solution.stepwise_states(to_merge=SolutionMerge.NODES) + time = solution.stepwise_time(to_merge=SolutionMerge.NODES).T[0] + bounds, bounds_time = self.get_bounds(solution) if show_bounds else None, None + stim_time = solution.ocp.nlp[0].model.stim_time if show_stim else None + + # Extract data from solution + cn = states["Cn"][0] + force = states["F"][0] + if solution.ocp.nlp[0].model._with_fatigue: + a = states["A"][0] + km = states["Km"][0] + tau1 = states["Tau1"][0] + + fatigue_model_used = solution.ocp.nlp[0].model._with_fatigue + # Create subplots + nrows = 2 if fatigue_model_used else 1 + fig, axs = plt.subplots(nrows, 1, figsize=(12, 9)) + + if nrows == 1: + axs = [axs] + + # --- Force Model --- # + ax1 = axs[0] + # Plot Cn + line_cn, = ax1.plot(time, cn, color="royalblue", label="Cn", lw=2) + ax1.set_ylabel("Cn (-)", fontsize=12, color="royalblue") + ax1.set_xlabel("Time (s)", fontsize=12) + ax1.set_title("Force model", fontsize=14, fontweight="bold") + ax1.tick_params(axis='y', colors="royalblue") + ax1.grid(True, linestyle="--", alpha=0.7) + + # Create twin for Force + ax1_force, line_force = self.create_twin_axes( + ax1, time, force, label="Force", + color="darkred", lw=3, + ylabel="Force (N)", tick_color="darkred" + ) + + if show_stim: + for stim in stim_time: + ax1.axvline(stim, color="goldenrod", linestyle="--", lw=1) + offset = 0.01 * (stim_time[-1] - stim_time[0]) / (stim_time[-1] - stim_time[0]) + ax1.text(stim_time[0]-offset, 0, "Stimulation", rotation=90, color="goldenrod", fontsize=10) + if show_bounds: + ax1.fill_between(bounds_time, bounds["cn"][0], bounds["cn"][1], color="royalblue", alpha=0.2) + ax1_force.fill_between(bounds_time, bounds["force"][0], bounds["force"][1], color="darkred", alpha=0.2) + + # Combine legends from ax1 and its twin + lines = [line_cn, line_force] + labels = [line.get_label() for line in lines] + ax1.legend(lines, labels, fontsize=10, fancybox=True, shadow=True) + + # --- Fatigue Model --- # + if fatigue_model_used: + ax2 = axs[1] + # Plot A on ax2 + line_a, = ax2.plot(time, a, color="forestgreen", label="A", lw=3) + ax2.set_ylabel("A (-)", fontsize=12, color="forestgreen") + ax2.set_xlabel("Time (s)", fontsize=12) + ax2.set_title("Fatigue model", fontsize=14, fontweight="bold") + ax2.tick_params(axis='y', colors="forestgreen") + ax2.grid(True, linestyle="--", alpha=0.7) + + # Create twin for Km and Tau1 + ax2_twin = ax2.twinx() + ax2_twin.spines["left"].set_color("crimson") + ax2_twin.yaxis.set_label_position('left') + ax2_twin.yaxis.set_ticks_position('left') + line_km, = ax2_twin.plot(time, km, color="crimson", label="Km", lw=3) + line_tau1, = ax2_twin.plot(time, tau1, color="purple", label="Tau1", lw=3) + ax2_twin.set_ylabel("Km (-) & Tau1 (s)", fontsize=12, color="crimson") + ax2_twin.tick_params(axis='y', colors="crimson") + self.axes_settings([ax2_twin]) + + # Combine legends for the fatigue model subplot + lines2 = [line_a, line_km, line_tau1] + labels2 = [line.get_label() for line in lines2] + ax2.legend(lines2, labels2, fontsize=10, fancybox=True, shadow=True) + + if show_bounds: + ax2.fill_between(bounds_time, bounds["a"][0], bounds["a"][1], color="forestgreen", alpha=0.2) + ax2_twin.fill_between(bounds_time, bounds["km"][0], bounds["km"][1], color="crimson", alpha=0.2) + ax2_twin.fill_between(bounds_time, bounds["tau1"][0], bounds["tau1"][1], color="purple", alpha=0.2) + + fig.suptitle(title, fontsize=16, fontweight="bold") + plt.tight_layout(rect=(0, 0, 1, 0.96)) + plt.show() + + def ivp_plot(self, title: str = None): + """ + Plot the force and (optionally) fatigue models. + + Parameters: + title (str, optional): Title for the figure. + """ + result = self.data + time = result["time"] + # Extract data from result + cn = result["Cn"][0] + force = result["F"][0] + fatigue_model_used = "A" in result + + if fatigue_model_used: + a = result["A"][0] + km = result["Km"][0] + tau1 = result["Tau1"][0] + + # Create subplots + nrows = 2 if fatigue_model_used else 1 + fig, axs = plt.subplots(nrows, 1, figsize=(12, 9)) + + if nrows == 1: + axs = [axs] + + # --- Force Model --- # + ax1 = axs[0] + # Plot Cn + line_cn, = ax1.plot(time, cn, color="royalblue", label="Cn", lw=2) + ax1.set_ylabel("Cn (-)", fontsize=12, color="royalblue") + ax1.set_xlabel("Time (s)", fontsize=12) + ax1.set_title("Force model", fontsize=14, fontweight="bold") + ax1.tick_params(axis='y', colors="royalblue") + ax1.grid(True, linestyle="--", alpha=0.7) + + # Create twin for Force + ax1_force, line_force = self.create_twin_axes( + ax1, time, force, label="Force", + color="darkred", lw=3, + ylabel="Force (N)", tick_color="darkred" + ) + + # Combine legends from ax1 and its twin + lines = [line_cn, line_force] + labels = [line.get_label() for line in lines] + ax1.legend(lines, labels, fontsize=10, fancybox=True, shadow=True) + + # --- Fatigue Model --- # + if fatigue_model_used: + ax2 = axs[1] + # Plot A on ax2 + line_a, = ax2.plot(time, a, color="forestgreen", label="A", lw=3) + ax2.set_ylabel("A (-)", fontsize=12, color="forestgreen") + ax2.set_xlabel("Time (s)", fontsize=12) + ax2.set_title("Fatigue model", fontsize=14, fontweight="bold") + ax2.tick_params(axis='y', colors="forestgreen") + ax2.grid(True, linestyle="--", alpha=0.7) + + # Create twin for Km and Tau1 + ax2_twin = ax2.twinx() + ax2_twin.spines["left"].set_color("crimson") + ax2_twin.yaxis.set_label_position('left') + ax2_twin.yaxis.set_ticks_position('left') + line_km, = ax2_twin.plot(time, km, color="crimson", label="Km", lw=3) + line_tau1, = ax2_twin.plot(time, tau1, color="purple", label="Tau1", lw=3) + ax2_twin.set_ylabel("Km (-) & Tau1 (s)", fontsize=12, color="crimson") + ax2_twin.tick_params(axis='y', colors="crimson") + self.axes_settings([ax2_twin]) + + # Combine legends for the fatigue model subplot + lines2 = [line_a, line_km, line_tau1] + labels2 = [line.get_label() for line in lines2] + ax2.legend(lines2, labels2, fontsize=10, fancybox=True, shadow=True) + + fig.suptitle(title, fontsize=16, fontweight="bold") + plt.tight_layout(rect=(0, 0, 1, 0.96)) + plt.show() + + def msk_plot(self, title: str = None, show_stim: bool = True, show_bounds: bool = True): + solution = self.data + self.force_keys = [key for key in solution.stepwise_states().keys() if key.startswith("F_")] + q, qdot, tau, force, time = self.get_data(solution) + bounds = self.get_msk_bounds(solution) if show_bounds else None + + fig, axs = plt.subplots(2, 2, figsize=(12, 9)) + + # Q states + q_keys = solution.ocp.nlp[0].model.bio_model.name_dof + q_keys = ["q_" + key for key in q_keys] + self.build_several_y_axis(axs[0, 0], time, q, labels=q_keys) + # Set titles/labels + axs[0, 0].set_title("Joint angles (rad)") + + # Qdot states + qdot_keys = solution.ocp.nlp[0].model.bio_model.name_dof + qdot_keys = ["qdot_" + key for key in qdot_keys] + self.build_several_y_axis(axs[0, 1], time, qdot, labels=qdot_keys) + # Set titles/labels + axs[0, 1].set_title("Joint velocity (rad/s)") + + # Tau + tau_keys = solution.ocp.nlp[0].model.bio_model.name_dof + tau_keys = ["tau_" + key for key in tau_keys] + tau_time = solution.decision_time(to_merge=SolutionMerge.NODES).T[0][:-1] + self.build_several_y_axis(axs[1, 0], tau_time, tau, labels=tau_keys) + # Set titles/labels + axs[1, 0].set_title("Torque (N.m)") + + # Forces + stim_time = np.array(solution.ocp.nlp[0].model.muscles_dynamics_model[0].stim_time) + stim_values = solution.parameters + # Todo normalize stim_value to yaxis scale + + force_label = "FES forces (N)" + + fes_parameter_label = "Pulse duration (us)" if isinstance(solution.ocp.nlp[0].model.muscles_dynamics_model[0], DingModelPulseWidthFrequency) else "Pulse intensity (mA)" + axes_title = [force_label, fes_parameter_label] + self.build_several_y_axis_FES(axs[1, 1], time, force, labels=self.force_keys, stim_time=stim_time, stim_values=stim_values, axes_title=axes_title) + + plt.legend() + plt.tight_layout() + plt.show() + + + +