From c8a45d4432b12a1d15f4bcb0b9552d03bb1987ca Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Wed, 9 Oct 2024 17:17:20 -0400 Subject: [PATCH 01/32] feat(external forces): implementing external forces into msk model --- ...timization_msk_dyn_with_external_forces.py | 43 ++++++++++ cocofest/models/dynamical_model.py | 39 ++++++++-- cocofest/optimization/fes_ocp_dynamics.py | 78 ++++++++++++++++--- 3 files changed, 144 insertions(+), 16 deletions(-) create mode 100644 cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces.py 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..6a24eeb --- /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 cocofest import DingModelPulseDurationFrequencyWithFatigue, OcpFesMsk, FesMskModel + +model = FesMskModel( + name=None, + biorbd_path="../msk_models/arm26_biceps_1dof.bioMod", + muscles_model=[DingModelPulseDurationFrequencyWithFatigue(muscle_name="BIClong")], + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=True, + segments_to_apply_external_forces=["r_ulna_radius_hand"], +) + +minimum_pulse_duration = DingModelPulseDurationFrequencyWithFatigue().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], + n_shooting=100, + final_time=1, + pulse_duration={ + "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={"Global": True, "Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, 0]), "force": np.array([0, 10, 0]), "point_of_application": np.array([0, 0, 0])}, +) + +sol = ocp.solve() +sol.animate() +sol.graphs(show_bounds=False) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index df47912..4a320b3 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -1,7 +1,7 @@ from typing import Callable import numpy as np -from casadi import vertcat, MX, SX +from casadi import vertcat, MX, SX, horzcat from bioptim import ( BiorbdModel, OptimalControlProgram, @@ -30,6 +30,7 @@ def __init__( activate_force_length_relationship: bool = False, activate_force_velocity_relationship: bool = False, activate_residual_torque: bool = False, + segments_to_apply_external_forces: list[str] = None, ): """ The custom model that will be used in the optimal control program for the FES-MSK models @@ -49,10 +50,9 @@ def __init__( activate_residual_torque: bool If the residual torque should be activated """ - - super().__init__(biorbd_path) + super().__init__(biorbd_path, segments_to_apply_external_forces=segments_to_apply_external_forces) + self.bio_model = BiorbdModel(biorbd_path, segments_to_apply_external_forces=segments_to_apply_external_forces) self._name = name - self.bio_model = BiorbdModel(biorbd_path) self._model_sanity( muscles_model, @@ -110,6 +110,7 @@ def muscle_dynamic( nlp: NonLinearProgram, muscle_models: list[FesModel], state_name_list=None, + external_forces=None, ) -> DynamicsEvaluation: """ The custom dynamics function that provides the derivative of the states: dxdt = f(t, x, u, p, s) @@ -134,6 +135,8 @@ def muscle_dynamic( The list of the muscle models state_name_list: list[str] The states names list + external_forces: MX | SX + The external forces acting on the system Returns ------- The derivative of the states in the tuple[MX | SX] format @@ -160,7 +163,7 @@ def muscle_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau - ddq = nlp.model.forward_dynamics(q, qdot, total_torque) + ddq = nlp.model.forward_dynamics(q=q, qdot=qdot, tau=total_torque, external_forces=external_forces) dxdt = vertcat(dxdt_muscle_list, dq, ddq) @@ -271,6 +274,15 @@ def declare_model_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ + external_forces = None + if numerical_data_timeseries is not None: + for key in numerical_data_timeseries.keys(): + if key == "external_forces": + _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) + external_forces = nlp.numerical_timeseries[0].mx + for i in range(1, numerical_data_timeseries[key].shape[1]): + external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].mx) + 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") @@ -289,6 +301,7 @@ def declare_model_variables( dyn_func=self.muscle_dynamic, muscle_models=self.muscles_dynamics_model, state_name_list=state_name_list, + external_forces=external_forces, ) @staticmethod @@ -314,3 +327,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/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 80070a4..e5c3a17 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -96,7 +96,11 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: msk_info["custom_constraint"], ) - dynamics = OcpFesMsk._declare_dynamics(input_dict["model"]) + external_forces = None + if input_dict["external_forces"]: + external_forces = OcpFesMsk._prepare_external_force(input_dict["external_forces"], input_dict["n_shooting"], input_dict["model"]) + + dynamics = OcpFesMsk._declare_dynamics(input_dict["model"], external_forces) initial_state = ( get_initial_guess( input_dict["model"].path, @@ -165,6 +169,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. @@ -204,6 +209,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 ------- @@ -226,6 +233,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) @@ -300,16 +308,64 @@ def _fill_msk_dict(pulse_duration, pulse_intensity, objective, msk_info): return pulse_duration, pulse_intensity, objective, msk_info @staticmethod - def _declare_dynamics(bio_models): - dynamics = DynamicsList() - dynamics.add( - bio_models.declare_model_variables, - dynamic_function=bio_models.muscle_dynamic, - expand_dynamics=True, - expand_continuity=False, - phase=0, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - ) + def _prepare_external_force(external_forces, n_shooting, model): + # Building the external forces array + external_forces_array = np.zeros((9, 1, n_shooting + 1)) + + if isinstance(external_forces["point_of_application"], str): + raise ValueError("Work in progess") + # marker_idx = model.bio_model.marker_index(external_forces["point_of_application"]) + + # Filling the external forces array for torque, force, and point of application + external_forces_array = OcpFesMsk.fill_external_forces(external_forces_array, external_forces["torque"], [0, 1, 2], n_shooting, "torque") + external_forces_array = OcpFesMsk.fill_external_forces(external_forces_array, external_forces["force"], [3, 4, 5], n_shooting, "force") + external_forces_array = OcpFesMsk.fill_external_forces(external_forces_array, external_forces["point_of_application"], [6, 7, 8], n_shooting, + "point of application") + + return external_forces_array + + @staticmethod + def fill_external_forces(array, values, indices, n_shooting, label): + # The external forces are defined as a 3D array with the following structure: + # - The first dimension is the torque, force and point of application (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz) + # - The second dimension is the number of external forces to apply (here by default 1) + # - The third dimension is the number of shooting points + if values.shape == (3,): + for i in range(3): + array[indices[i], 0, :] = np.array([values[i]] * (n_shooting + 1)) + else: + if values.shape[1] != n_shooting + 1: + raise ValueError( + f"The number of shooting points + 1 must be the same as the number of given {label} values") + for i in range(3): + array[indices[i], 0, :] = np.array(values[i]) + + return array + + @staticmethod + def _declare_dynamics(bio_models, external_forces): + if isinstance(external_forces, np.ndarray): + dynamics = DynamicsList() + dynamics.add( + bio_models.declare_model_variables, + dynamic_function=bio_models.muscle_dynamic, + expand_dynamics=True, + expand_continuity=False, + phase=0, + phase_dynamics=PhaseDynamics.ONE_PER_NODE, + numerical_data_timeseries={"external_forces": external_forces}, + ) + + else: + dynamics = DynamicsList() + dynamics.add( + bio_models.declare_model_variables, + dynamic_function=bio_models.muscle_dynamic, + expand_dynamics=True, + expand_continuity=False, + phase=0, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + ) return dynamics @staticmethod From 63df8755736d52a9f6e86946d0f7684199fff0e7 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Thu, 10 Oct 2024 19:31:51 -0400 Subject: [PATCH 02/32] feat(external forces) last node control fix and functional external F --- ...se_duration_optimization_msk_dyn_with_external_forces.py | 4 ++-- .../pulse_duration_optimization_musculoskeletal_dynamic.py | 6 +++--- cocofest/optimization/fes_ocp_dynamics.py | 6 ++++-- 3 files changed, 9 insertions(+), 7 deletions(-) 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 index 6a24eeb..c99847b 100644 --- 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 @@ -35,9 +35,9 @@ "bound_type": "start_end", "bound_data": [[5], [120]], "with_residual_torque": True}, - external_forces={"Global": True, "Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, 0]), "force": np.array([0, 10, 0]), "point_of_application": np.array([0, 0, 0])}, + external_forces={"Global": True, "Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, 0]), "force": np.array([100, 0, 100]), "point_of_application": np.array([0, 0, 0])}, ) sol = ocp.solve() -sol.animate() +# sol.animate() sol.graphs(show_bounds=False) 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 5ef0ced..ff88792 100644 --- a/cocofest/examples/getting_started/pulse_duration_optimization_musculoskeletal_dynamic.py +++ b/cocofest/examples/getting_started/pulse_duration_optimization_musculoskeletal_dynamic.py @@ -4,7 +4,7 @@ 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. """ - +from bioptim import Solver from cocofest import DingModelPulseDurationFrequencyWithFatigue, OcpFesMsk, FesMskModel model = FesMskModel( @@ -35,6 +35,6 @@ }, ) -sol = ocp.solve() -sol.animate() +sol = ocp.solve(Solver.IPOPT(_max_iter=2000)) +# sol.animate() sol.graphs(show_bounds=False) diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index e5c3a17..62147b9 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -94,6 +94,7 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: input_dict["stim_time"], input_dict["control_type"], msk_info["custom_constraint"], + input_dict["external_forces"], ) external_forces = None @@ -531,13 +532,14 @@ 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, stim_time, control_type, custom_constraint=None, external_forces=None): constraints = ConstraintList() if model.activate_residual_torque: + 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), ) From 5d927a62fc1ab0a486f1dd4b10b86c7c01d9e020 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 15 Oct 2024 18:31:10 -0400 Subject: [PATCH 03/32] feat(pedal): trying to implement a pedal with 2 additional dof --- ...on_msk_dyn_with_external_forces_cycling.py | 61 +++ .../msk_models/arm26_cycling_pedal.bioMod | 417 ++++++++++++++++++ .../arm26_cycling_pedal_aligned.bioMod | 412 +++++++++++++++++ .../simplified_UL_Seth_pedal_aligned.bioMod | 393 +++++++++++++++++ cocofest/models/dynamical_model.py | 76 ++++ cocofest/optimization/fes_ocp_dynamics.py | 49 +- 6 files changed, 1407 insertions(+), 1 deletion(-) create mode 100644 cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces_cycling.py create mode 100644 cocofest/examples/msk_models/arm26_cycling_pedal.bioMod create mode 100644 cocofest/examples/msk_models/arm26_cycling_pedal_aligned.bioMod create mode 100644 cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod 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..a257f64 --- /dev/null +++ b/cocofest/examples/getting_started/pulse_duration_optimization_msk_dyn_with_external_forces_cycling.py @@ -0,0 +1,61 @@ +""" +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 DingModelPulseDurationFrequencyWithFatigue, OcpFesMsk, FesMskModel, SolutionToPickle, PickleAnimate +import biorbd + +model = FesMskModel( + name=None, + # biorbd_path="../msk_models/arm26_cycling_pedal_aligned.bioMod", + biorbd_path="../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", + muscles_model=[ + DingModelPulseDurationFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A"), + DingModelPulseDurationFrequencyWithFatigue(muscle_name="DeltoideusScapula_P"), + DingModelPulseDurationFrequencyWithFatigue(muscle_name="TRIlong"), + DingModelPulseDurationFrequencyWithFatigue(muscle_name="BIC_long"), + DingModelPulseDurationFrequencyWithFatigue(muscle_name="BIC_brevis"), + ], + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=True, + # segments_to_apply_external_forces=["r_ulna_radius_hand"], +) + +minimum_pulse_duration = DingModelPulseDurationFrequencyWithFatigue().pd0 +ocp = OcpFesMsk.prepare_ocp( + model=model, + stim_time=list(np.round(np.linspace(0, 1, 11), 3))[:-1], + n_shooting=100, + final_time=1, + pulse_duration={ + "min": minimum_pulse_duration, + "max": 0.0006, + "bimapping": False, + }, + msk_info={"with_residual_torque": True}, + objective={ + "cycling": {"x_center": 0.3, "y_center": 0, "radius": 0.1, "target": "marker"}, + "minimize_residual_torque": True, + "minimize_muscle_force": True, + }, + warm_start=False, + n_threads=5, + # external_forces={"Global": True, "Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, -5]), "force": np.array([0, 0, 0]), "point_of_application": np.array([0, 0, 0])}, +) + +# sol = ocp.solve(solver=Solver.IPOPT(_max_iter=1)) +sol = ocp.solve() +# SolutionToPickle(sol, "oui.pkl", "").pickle() +# biorbd_model = biorbd.Model("../msk_models/arm26_cycling_pedal.bioMod") +# PickleAnimate("oui.pkl").animate(model=biorbd_model) + + +sol.animate(show_tracked_markers=True) +# sol.animate(viewer="pyorerun") +sol.graphs(show_bounds=False) 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..6af3e32 --- /dev/null +++ b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod @@ -0,0 +1,393 @@ +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.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.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 -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 + + // 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/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 4a320b3..24b397b 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -255,10 +255,81 @@ 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, + external_forces: 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 + external_forces: list[Any] + The external forces + 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, + nlp.states, + 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) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau + + return nlp.model.contact_forces(q, qdot, tau, external_forces) + def declare_model_variables( self, ocp: OptimalControlProgram, nlp: NonLinearProgram, + with_contact: bool = False, numerical_data_timeseries: dict[str, np.ndarray] = None, ): """ @@ -304,6 +375,11 @@ def declare_model_variables( external_forces=external_forces, ) + if with_contact: + ConfigureProblem.configure_contact_function( + ocp, nlp, self.forces_from_fes_driven, external_forces=external_forces + ) + @staticmethod def _model_sanity( muscles_model, diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 62147b9..6545dbd 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -355,6 +355,7 @@ def _declare_dynamics(bio_models, external_forces): phase=0, phase_dynamics=PhaseDynamics.ONE_PER_NODE, numerical_data_timeseries={"external_forces": external_forces}, + with_contact=True, ) else: @@ -366,6 +367,7 @@ def _declare_dynamics(bio_models, external_forces): expand_continuity=False, phase=0, phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + with_contact=True, ) return dynamics @@ -587,6 +589,42 @@ def _build_constraints(model, n_shooting, final_time, stim_time, control_type, c for j in range(len(custom_constraint[i])): constraints.add(custom_constraint[i][j]) + # wheel_target = np.linspace(np.pi, -np.pi, n_shooting + 1)[np.newaxis, :] + # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=Node.ALL, target=wheel_target) + # constraints.add( + # ConstraintFcn.SUPERIMPOSE_MARKERS, + # node=Node.ALL, + # first_marker="wheel_center", + # second_marker="global_wheel_center", + # axes=[Axis.X, Axis.Y], + # ) + + constraints.add( + ConstraintFcn.TRACK_CONTACT_FORCES, + min_bound=-500, + max_bound=500, + node=Node.ALL_SHOOTING, + contact_index=1, + ) + + # x_center = 0.3 + # y_center = 0 + # radius = 0.1 + # 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) + # ] + # ) + # constraints.add( + # ConstraintFcn.TRACK_MARKERS, + # axes=[Axis.X, Axis.Y], + # marker_index=0, + # target=circle_coord_list.T, + # node=Node.ALL, + # phase=0, + # ) + return constraints @staticmethod @@ -714,6 +752,7 @@ def _set_u_bounds(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 = [-50] * nb_tau, [50] * nb_tau, [0] * nb_tau + tau_min[-1], tau_max[-1] = 0, 0 u_bounds.add( key="tau", min_bound=tau_min, max_bound=tau_max, phase=0, interpolation=InterpolationType.CONSTANT ) @@ -805,7 +844,7 @@ def _set_objective( weight=10000000, axes=[Axis.X, Axis.Y], marker_index=0, - target=circle_coord_list.T, + target=circle_coord_list[:, np.newaxis].T, node=Node.ALL, phase=0, quadratic=True, @@ -866,6 +905,14 @@ def _set_objective( phase=0, ) + # objective_functions.add( + # ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, + # node=Node.ALL, + # first_marker="wheel", + # second_marker="COM_hand", + # axes=[Axis.X, Axis.Y], + # ) + return objective_functions @staticmethod From ee59847e5672fa62aea1f30124e8d8915566d2d8 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 22 Nov 2024 17:20:07 -0500 Subject: [PATCH 04/32] feat(external_force): example with external torque for a biceps curl --- ...timization_msk_dyn_with_external_forces.py | 5 +++-- cocofest/models/dynamical_model.py | 20 ++++--------------- cocofest/optimization/fes_ocp_dynamics.py | 3 +-- 3 files changed, 8 insertions(+), 20 deletions(-) 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 index 4137625..40a84de 100644 --- 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 @@ -6,6 +6,7 @@ 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 @@ -34,9 +35,9 @@ "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, 0]), "with_contact": False}, + external_forces={"Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, -1]), "with_contact": False}, ) -sol = ocp.solve() +sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=2000)) sol.animate() sol.graphs(show_bounds=False) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 7afc7d7..d83719e 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -147,8 +147,6 @@ def muscle_dynamic( The derivative of the states in the tuple[MX | SX] format """ - if external_forces is None: - external_forces = [] q = DynamicsFunctions.get(nlp.states["q"], states) qdot = DynamicsFunctions.get(nlp.states["qdot"], states) tau = DynamicsFunctions.get(nlp.controls["tau"], controls) if "tau" in nlp.controls.keys() else 0 @@ -170,7 +168,7 @@ def muscle_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau - external_forces = [] if external_forces is None else external_forces + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_data_timeseries) ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, total_torque, external_forces, parameters) # q, qdot, tau, external_forces, parameters # TODO: Add the with_contact=True when the contact forces will be added dxdt = vertcat(dxdt_muscle_list, dq, ddq) @@ -285,7 +283,6 @@ def forces_from_fes_driven( nlp, with_passive_torque: bool = False, with_ligament: bool = False, - external_forces: list = None, state_name_list=None, ) -> MX: """ @@ -311,8 +308,8 @@ def forces_from_fes_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - external_forces: list[Any] - The external forces + state_name_list: list[str] + The states names list Returns ---------- MX.sym @@ -367,14 +364,6 @@ def declare_model_variables( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ - external_forces = None - if numerical_data_timeseries is not None: - for key in numerical_data_timeseries.keys(): - if key == "external_forces": - _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].cx - for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) 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) @@ -394,12 +383,11 @@ def declare_model_variables( dyn_func=self.muscle_dynamic, muscle_models=self.muscles_dynamics_model, state_name_list=state_name_list, - external_forces=external_forces, ) if with_contact: ConfigureProblem.configure_contact_function( - ocp, nlp, self.forces_from_fes_driven, external_forces=external_forces, state_name_list=state_name_list + ocp, nlp, self.forces_from_fes_driven, state_name_list=state_name_list ) @staticmethod diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 5b7b254..a8ac5c3 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -549,7 +549,7 @@ def _build_parameters( def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None): constraints = ConstraintList() - 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, @@ -769,7 +769,6 @@ def _set_u_bounds(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 = [-50] * nb_tau, [50] * nb_tau, [0] * nb_tau - tau_min[-1], tau_max[-1] = 0, 0 u_bounds.add( key="tau", min_bound=tau_min, max_bound=tau_max, phase=0, interpolation=InterpolationType.CONSTANT ) From a37b367ceb1925ae3e3e8fd8db659c9e574271ac Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 22 Nov 2024 18:12:07 -0500 Subject: [PATCH 05/32] feat(external_force): cycling example (optimal solution found but wrong) --- ...on_msk_dyn_with_external_forces_cycling.py | 34 +++++++++---------- cocofest/models/dynamical_model.py | 3 +- cocofest/optimization/fes_ocp_dynamics.py | 22 +++++++++++- 3 files changed, 39 insertions(+), 20 deletions(-) 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 index a257f64..7fd25d9 100644 --- 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 @@ -7,7 +7,7 @@ """ import numpy as np from bioptim import Solver -from cocofest import DingModelPulseDurationFrequencyWithFatigue, OcpFesMsk, FesMskModel, SolutionToPickle, PickleAnimate +from cocofest import DingModelPulseWidthFrequencyWithFatigue, OcpFesMsk, FesMskModel, SolutionToPickle, PickleAnimate import biorbd model = FesMskModel( @@ -15,26 +15,25 @@ # biorbd_path="../msk_models/arm26_cycling_pedal_aligned.bioMod", biorbd_path="../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", muscles_model=[ - DingModelPulseDurationFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A"), - DingModelPulseDurationFrequencyWithFatigue(muscle_name="DeltoideusScapula_P"), - DingModelPulseDurationFrequencyWithFatigue(muscle_name="TRIlong"), - DingModelPulseDurationFrequencyWithFatigue(muscle_name="BIC_long"), - DingModelPulseDurationFrequencyWithFatigue(muscle_name="BIC_brevis"), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A", is_approximated=True), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusScapula_P", is_approximated=True), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="TRIlong", is_approximated=True), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_long", is_approximated=True), + DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_brevis", is_approximated=True), ], activate_force_length_relationship=True, activate_force_velocity_relationship=True, activate_residual_torque=True, - # segments_to_apply_external_forces=["r_ulna_radius_hand"], + external_force_set=None, # External forces will be added ) -minimum_pulse_duration = DingModelPulseDurationFrequencyWithFatigue().pd0 +minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 ocp = OcpFesMsk.prepare_ocp( model=model, stim_time=list(np.round(np.linspace(0, 1, 11), 3))[:-1], - n_shooting=100, final_time=1, - pulse_duration={ - "min": minimum_pulse_duration, + pulse_width={ + "min": minimum_pulse_width, "max": 0.0006, "bimapping": False, }, @@ -44,16 +43,15 @@ "minimize_residual_torque": True, "minimize_muscle_force": True, }, - warm_start=False, - n_threads=5, - # external_forces={"Global": True, "Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, -5]), "force": np.array([0, 0, 0]), "point_of_application": np.array([0, 0, 0])}, + initial_guess_warm_start=False, + external_forces={"Segment_application": "r_ulna_radius_hand", "torque": np.array([0, 0, -1]), "with_contact": True}, ) # sol = ocp.solve(solver=Solver.IPOPT(_max_iter=1)) -sol = ocp.solve() -# SolutionToPickle(sol, "oui.pkl", "").pickle() -# biorbd_model = biorbd.Model("../msk_models/arm26_cycling_pedal.bioMod") -# PickleAnimate("oui.pkl").animate(model=biorbd_model) +sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) +SolutionToPickle(sol, "hand_cycling_external_forces.pkl", "").pickle() +biorbd_model = biorbd.Model("../msk_models/simplified_UL_Seth_pedal_aligned.bioMod") +PickleAnimate("hand_cycling_external_forces.pkl").animate(model=biorbd_model) sol.animate(show_tracked_markers=True) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index d83719e..7779764 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -169,8 +169,9 @@ def muscle_dynamic( dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_data_timeseries) + with_contact = True if nlp.model.bio_model.contact_names != () else False ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, total_torque, external_forces, parameters) # q, qdot, tau, external_forces, parameters - # TODO: Add the with_contact=True when the contact forces will be added + # TODO: Add a better way of with_contact=True dxdt = vertcat(dxdt_muscle_list, dq, ddq) return DynamicsEvaluation(dxdt=dxdt, defects=None) diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index a8ac5c3..0a00fa6 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -94,6 +94,7 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: input_dict["control_type"], msk_info["custom_constraint"], input_dict["external_forces"], + cycling=True if "cycling" in objective.keys() else False, ) numerical_time_series = None @@ -546,7 +547,7 @@ def _build_parameters( ) @staticmethod - def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None): + def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None, cycling=False): constraints = ConstraintList() if model.activate_residual_torque and control_type == ControlType.LINEAR_CONTINUOUS: @@ -594,6 +595,25 @@ def _build_constraints(model, n_shooting, final_time, stim_time, control_type, c model_idx=i, ) + cycling_with_contact = False if external_forces is None else cycling and external_forces["with_contact"] + + if cycling_with_contact: + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=Node.ALL, + marker_index=model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + min_bound=np.array([0, 0]), + max_bound=np.array([0.05 ** 2, 0.05 ** 2]), + ) + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=Node.ALL, + axes=[Axis.X, Axis.Y], + ) + if custom_constraint: for i in range(len(custom_constraint)): if custom_constraint[i]: From fb2b95de9eb2ac9ab6295ee907f16e93b6f858ce Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 26 Nov 2024 16:32:41 -0500 Subject: [PATCH 06/32] feat(external_force): added example of cycling with ext_f and contact --- .../inverse_kinematics_and_dynamics.py | 4 +- .../dynamics/cycling/cycling_torque_driven.py | 4 +- ...on_msk_dyn_with_external_forces_cycling.py | 27 +- cocofest/models/dynamical_model.py | 2 +- cocofest/optimization/fes_ocp_dynamics.py | 403 +++++++++--------- 5 files changed, 230 insertions(+), 210 deletions(-) diff --git a/cocofest/dynamics/inverse_kinematics_and_dynamics.py b/cocofest/dynamics/inverse_kinematics_and_dynamics.py index 243bc54..0c67d19 100644 --- a/cocofest/dynamics/inverse_kinematics_and_dynamics.py +++ b/cocofest/dynamics/inverse_kinematics_and_dynamics.py @@ -91,8 +91,8 @@ def inverse_kinematics_cycling( 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.") x_y_z_coord = np.array( diff --git a/cocofest/examples/dynamics/cycling/cycling_torque_driven.py b/cocofest/examples/dynamics/cycling/cycling_torque_driven.py index 3087710..55f4d8f 100644 --- a/cocofest/examples/dynamics/cycling/cycling_torque_driven.py +++ b/cocofest/examples/dynamics/cycling/cycling_torque_driven.py @@ -94,7 +94,7 @@ def prepare_ocp( u_init.add("tau", u_guess, interpolation=InterpolationType.EACH_FRAME) return OptimalControlProgram( - bio_model, + [bio_model], dynamics, n_shooting, final_time, @@ -119,7 +119,7 @@ def main(): ) ocp.add_plot_penalty(CostType.ALL) sol = ocp.solve() - sol.animate() + sol.animate(viewer="pyorerun") sol.graphs() 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 index 7fd25d9..ccbb7b0 100644 --- 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 @@ -6,20 +6,19 @@ External forces will be applied to the system to simulate a real-world scenario. """ import numpy as np -from bioptim import Solver +from bioptim import Solver, ControlType from cocofest import DingModelPulseWidthFrequencyWithFatigue, OcpFesMsk, FesMskModel, SolutionToPickle, PickleAnimate import biorbd model = FesMskModel( name=None, - # biorbd_path="../msk_models/arm26_cycling_pedal_aligned.bioMod", - biorbd_path="../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", + biorbd_path="../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod", muscles_model=[ - DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusClavicle_A", is_approximated=True), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="DeltoideusScapula_P", is_approximated=True), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="TRIlong", is_approximated=True), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_long", is_approximated=True), - DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIC_brevis", is_approximated=True), + 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, @@ -28,7 +27,7 @@ ) minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 -ocp = OcpFesMsk.prepare_ocp( +ocp = OcpFesMsk.prepare_ocp_for_cycling( model=model, stim_time=list(np.round(np.linspace(0, 1, 11), 3))[:-1], final_time=1, @@ -39,21 +38,21 @@ }, msk_info={"with_residual_torque": True}, objective={ - "cycling": {"x_center": 0.3, "y_center": 0, "radius": 0.1, "target": "marker"}, + "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": "r_ulna_radius_hand", "torque": np.array([0, 0, -1]), "with_contact": True}, + external_forces={"Segment_application": "wheel", "torque": np.array([0, 0, 0]), "with_contact": True}, + control_type=ControlType.CONSTANT, ) # sol = ocp.solve(solver=Solver.IPOPT(_max_iter=1)) sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) SolutionToPickle(sol, "hand_cycling_external_forces.pkl", "").pickle() -biorbd_model = biorbd.Model("../msk_models/simplified_UL_Seth_pedal_aligned.bioMod") +biorbd_model = biorbd.Model("../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod") PickleAnimate("hand_cycling_external_forces.pkl").animate(model=biorbd_model) - sol.animate(show_tracked_markers=True) # sol.animate(viewer="pyorerun") -sol.graphs(show_bounds=False) +sol.graphs(show_bounds=True) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 7779764..66d133e 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -170,7 +170,7 @@ def muscle_dynamic( total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_data_timeseries) with_contact = True if nlp.model.bio_model.contact_names != () else False - ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, total_torque, external_forces, parameters) # q, qdot, tau, external_forces, parameters + ddq = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, total_torque, external_forces, parameters) # q, qdot, tau, external_forces, parameters # TODO: Add a better way of with_contact=True dxdt = vertcat(dxdt_muscle_list, dq, ddq) diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 0a00fa6..2773ea9 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -24,7 +24,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 @@ -32,7 +31,10 @@ from ..optimization.fes_ocp import OcpFes from ..fourier_approx import FourierSeries from ..custom_constraints import CustomConstraint -from ..setup_external_forces import setup_external_forces +from ..dynamics.inverse_kinematics_and_dynamics import ( + inverse_kinematics_cycling, + inverse_dynamics_cycling, +) class OcpFesMsk: @@ -94,43 +96,16 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: input_dict["control_type"], msk_info["custom_constraint"], input_dict["external_forces"], - cycling=True if "cycling" in objective.keys() else False, ) - numerical_time_series = None - external_force_set = None - with_contact = False - if input_dict["external_forces"]: - external_force_set = ExternalForceSetTimeSeries(nb_frames=input_dict["n_shooting"]) - reshape_values_array = np.tile(input_dict["external_forces"]["torque"][:, np.newaxis], (1, input_dict["n_shooting"])) - external_force_set.add_torque(segment=input_dict["external_forces"]["Segment_application"], - values=reshape_values_array) - - # Dynamics - 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 - + numerical_time_series, with_contact, external_force_set = OcpFesMsk._prepare_numerical_time_series(input_dict["n_shooting"], input_dict["external_forces"]) dynamics = OcpFesMsk._declare_dynamics(input_dict["model"], numerical_time_series=numerical_time_series, with_contact=with_contact) - 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 - ) - 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"]) + 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 @@ -282,6 +257,96 @@ def prepare_ocp( n_threads=optimization_dict["n_threads"], ) + @staticmethod + def prepare_ocp_for_cycling( + 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, + msk_info: dict = None, + use_sx: bool = True, + initial_guess_warm_start: bool = False, + ode_solver: OdeSolverBase = OdeSolver.RK4(n_integration_steps=1), + control_type: ControlType = ControlType.CONSTANT, + n_threads: int = 1, + external_forces: dict = None, + ): + input_dict = { + "model": model, + "stim_time": stim_time, + "n_shooting": OcpFes.prepare_n_shooting(stim_time, final_time), + "final_time": final_time, + "pulse_event": pulse_event, + "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 OptimalControlProgram( + bio_model=[optimization_dict["model"]], + dynamics=optimization_dict["dynamics"], + n_shooting=optimization_dict["n_shooting"], + phase_time=optimization_dict["final_time"], + 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"], + ) + + @staticmethod + def _prepare_optimization_problem_for_cycling(optimization_dict: dict, input_dict: dict) -> dict: + OcpFesMsk._check_if_cycling_objectives_are_feasible(input_dict["objective"]["cycling"], input_dict["model"]) + + q_guess, qdot_guess, u_guess = OcpFesMsk._prepare_initial_guess_cycling(input_dict["model"].biorbd_path, + input_dict["n_shooting"], + input_dict["objective"]["cycling"]["x_center"], + input_dict["objective"]["cycling"]["y_center"], + input_dict["objective"]["cycling"]["radius"]) + x_initial_guess = {"q_guess": q_guess, "qdot_guess": qdot_guess} + x_bounds, x_init = OcpFesMsk._set_bounds_msk_for_cycling(optimization_dict["x_bounds"], optimization_dict["x_init"], input_dict["model"], x_initial_guess) + + u_initial_guess = {"u_guess": u_guess} + input_dict["msk_info"]["with_residual_torque"] = input_dict["msk_info"]["with_residual_torque"] if "with_residual_torque" in input_dict["msk_info"].keys() else False + u_bounds, u_init = OcpFesMsk._set_u_bounds_msk_for_cycling(optimization_dict["u_bounds"], optimization_dict["u_init"], input_dict["model"], input_dict["msk_info"]["with_residual_torque"], u_initial_guess) + + if "with_contact" in input_dict["external_forces"] and input_dict["external_forces"]["with_contact"]: + constraints = OcpFesMsk._build_constraints_for_cycling(optimization_dict["constraints"], input_dict["model"]) + else: + constraints = optimization_dict["constraints"] + + optimization_dict_for_cycling = { + "x_init": x_init, + "x_bounds": x_bounds, + "u_init": u_init, + "u_bounds": u_bounds, + "constraints": constraints, + } + + return optimization_dict_for_cycling + @staticmethod def _fill_msk_dict(pulse_width, pulse_intensity, objective, msk_info): @@ -331,66 +396,36 @@ def _fill_msk_dict(pulse_width, pulse_intensity, objective, msk_info): return pulse_width, pulse_intensity, objective, msk_info @staticmethod - def _prepare_external_force(external_forces, n_shooting, model): - # Building the external forces array - external_forces_array = np.zeros((9, 1, n_shooting + 1)) - - if isinstance(external_forces["point_of_application"], str): - raise ValueError("Work in progess") - # marker_idx = model.bio_model.marker_index(external_forces["point_of_application"]) - - # Filling the external forces array for torque, force, and point of application - external_forces_array = OcpFesMsk.fill_external_forces(external_forces_array, external_forces["torque"], [0, 1, 2], n_shooting, "torque") - external_forces_array = OcpFesMsk.fill_external_forces(external_forces_array, external_forces["force"], [3, 4, 5], n_shooting, "force") - external_forces_array = OcpFesMsk.fill_external_forces(external_forces_array, external_forces["point_of_application"], [6, 7, 8], n_shooting, - "point of application") + def _prepare_numerical_time_series(n_shooting, external_forces): - return external_forces_array + if external_forces: + external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) + reshape_values_array = np.tile(external_forces["torque"][:, np.newaxis], (1, n_shooting)) + external_force_set.add_torque(segment=external_forces["Segment_application"], + values=reshape_values_array) - @staticmethod - def fill_external_forces(array, values, indices, n_shooting, label): - # The external forces are defined as a 3D array with the following structure: - # - The first dimension is the torque, force and point of application (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz) - # - The second dimension is the number of external forces to apply (here by default 1) - # - The third dimension is the number of shooting points - if values.shape == (3,): - for i in range(3): - array[indices[i], 0, :] = np.array([values[i]] * (n_shooting + 1)) + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + with_contact = external_forces["with_contact"] if "with_contact" in external_forces.keys() else False else: - if values.shape[1] != n_shooting + 1: - raise ValueError( - f"The number of shooting points + 1 must be the same as the number of given {label} values") - for i in range(3): - array[indices[i], 0, :] = np.array(values[i]) + numerical_time_series = None + with_contact = False + external_force_set = None - return array + return numerical_time_series, with_contact, external_force_set @staticmethod def _declare_dynamics(bio_models, numerical_time_series, with_contact): - if numerical_time_series: - dynamics = DynamicsList() - dynamics.add( - bio_models.declare_model_variables, - dynamic_function=bio_models.muscle_dynamic, - expand_dynamics=True, - expand_continuity=False, - phase=0, - phase_dynamics=PhaseDynamics.ONE_PER_NODE, - numerical_data_timeseries=numerical_time_series, - with_contact=with_contact, - ) - - else: - dynamics = DynamicsList() - dynamics.add( - bio_models.declare_model_variables, - dynamic_function=bio_models.muscle_dynamic, - expand_dynamics=True, - expand_continuity=False, - phase=0, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - with_contact=False, - ) + dynamics = DynamicsList() + dynamics.add( + bio_models.declare_model_variables, + dynamic_function=bio_models.muscle_dynamic, + expand_dynamics=True, + expand_continuity=False, + phase=0, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + numerical_data_timeseries=numerical_time_series, + with_contact=with_contact, + ) return dynamics @staticmethod @@ -547,7 +582,7 @@ def _build_parameters( ) @staticmethod - def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None, cycling=False): + def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None): #, cycling=False): constraints = ConstraintList() if model.activate_residual_torque and control_type == ControlType.LINEAR_CONTINUOUS: @@ -595,71 +630,43 @@ def _build_constraints(model, n_shooting, final_time, stim_time, control_type, c model_idx=i, ) - cycling_with_contact = False if external_forces is None else cycling and external_forces["with_contact"] - - if cycling_with_contact: - constraints.add( - ConstraintFcn.TRACK_MARKERS_VELOCITY, - node=Node.ALL, - marker_index=model.marker_index("wheel_center"), - axes=[Axis.X, Axis.Y], - min_bound=np.array([0, 0]), - max_bound=np.array([0.05 ** 2, 0.05 ** 2]), - ) - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - first_marker="wheel_center", - second_marker="global_wheel_center", - node=Node.ALL, - axes=[Axis.X, Axis.Y], - ) - if custom_constraint: for i in range(len(custom_constraint)): if custom_constraint[i]: for j in range(len(custom_constraint[i])): constraints.add(custom_constraint[i][j]) - # wheel_target = np.linspace(np.pi, -np.pi, n_shooting + 1)[np.newaxis, :] - # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=Node.ALL, target=wheel_target) - # constraints.add( - # ConstraintFcn.SUPERIMPOSE_MARKERS, - # node=Node.ALL, - # first_marker="wheel_center", - # second_marker="global_wheel_center", - # axes=[Axis.X, Axis.Y], - # ) - - # constraints.add( - # ConstraintFcn.TRACK_CONTACT_FORCES, - # min_bound=-500, - # max_bound=500, - # node=Node.ALL_SHOOTING, - # contact_index=1, - # ) - - # x_center = 0.3 - # y_center = 0 - # radius = 0.1 - # 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) - # ] - # ) - # constraints.add( - # ConstraintFcn.TRACK_MARKERS, - # axes=[Axis.X, Axis.Y], - # marker_index=0, - # target=circle_coord_list.T, - # node=Node.ALL, - # phase=0, - # ) + return constraints + @staticmethod + def _build_constraints_for_cycling(constraints, model): + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=Node.START, + marker_index=model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + ) + + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=Node.START, + axes=[Axis.X, Axis.Y], + ) return constraints @staticmethod - def _set_bounds(bio_models, msk_info, initial_state, n_cycles_simultaneous=1): + def _prepare_initial_guess_cycling(biorbd_model_path, n_shooting, x_center, y_center, radius): + biorbd_model_path = "../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" #TODO : make it a def entry + q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( + biorbd_model_path, n_shooting, x_center, y_center, radius, ik_method="trf" + ) + u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) + return q_guess, qdot_guess, u_guess + + @staticmethod + def _set_bounds_fes(bio_models): # ---- STATE BOUNDS REPRESENTATION ---- # # # |‾‾‾‾‾‾‾‾‾‾x_max_middle‾‾‾‾‾‾‾‾‾‾‾‾x_max_end‾ @@ -709,6 +716,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 = [] @@ -748,52 +759,29 @@ 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_bounds_msk_for_cycling(x_bounds, x_init, bio_models, initial_guess): + q_x_bounds = bio_models.bounds_from_ranges("q") + qdot_x_bounds = bio_models.bounds_from_ranges("qdot") + qdot_x_bounds.max[2] = [0, 0, 0] + + x_bounds.add(key="q", bounds=q_x_bounds, phase=0) + x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) + + if initial_guess["q_guess"] is not None: + x_init.add("q", initial_guess["q_guess"], interpolation=InterpolationType.EACH_FRAME) + if initial_guess["qdot_guess"] is not None: + x_init.add("qdot", initial_guess["qdot_guess"], interpolation=InterpolationType.EACH_FRAME) 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) @@ -808,6 +796,34 @@ 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 = [-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) + return u_bounds, u_init + + @staticmethod + def _set_u_bounds_msk_for_cycling(u_bounds, u_init, bio_models, with_residual_torque, u_initial_guess): + if with_residual_torque: + nb_tau = bio_models.nb_tau + tau_min, tau_max, tau_init = [-50] * nb_tau, [50] * nb_tau, [0] * nb_tau + tau_min[2] = 0 + tau_max[2] = 0 + 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 u_initial_guess["u_guess"] is not None: + u_init.add("tau", u_initial_guess["u_guess"], interpolation=InterpolationType.EACH_FRAME) + + return u_bounds, u_init + @staticmethod def _set_objective( n_shooting, @@ -944,14 +960,6 @@ def _set_objective( phase=0, ) - # objective_functions.add( - # ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, - # node=Node.ALL, - # first_marker="wheel", - # second_marker="COM_hand", - # axes=[Axis.X, Axis.Y], - # ) - return objective_functions @staticmethod @@ -1074,3 +1082,16 @@ def _sanity_check_msk_inputs( if list_to_check[i]: if not isinstance(list_to_check[i], bool): raise TypeError(f"{list_to_check_name[i]} should be a boolean") + + @staticmethod + def _check_if_cycling_objectives_are_feasible(cycling_objective, model): + if "x_center" in cycling_objective and "y_center" in cycling_objective and "radius" in cycling_objective: + with open(model.bio_model.path) as f: + lines = f.readlines() + check_x_center = True in ["$wheel_x_position " + str(cycling_objective["x_center"]) in line for line in lines] + check_y_center = True in ["$wheel_y_position " + str(cycling_objective["y_center"]) in line for line in lines] + check_radius = True in ["$wheel_radius " + str(cycling_objective["radius"]) in line for line in lines] + if not all([check_x_center, check_y_center, check_radius]): + raise ValueError("x_center, y_center and radius should be declared and have the same value in the model") + else: + raise ValueError("cycling_objective should contain x_center, y_center and radius keys") From 1708b6e28d8b142ea3c4df25119823209f183362 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 3 Dec 2024 09:26:29 -0500 Subject: [PATCH 07/32] feat (nmpc fes): some developpement for fes nmpc --- .../inverse_kinematics_and_dynamics.py | 25 ++++++++++----- ...on_msk_dyn_with_external_forces_cycling.py | 14 ++++----- .../simplified_UL_Seth_pedal_aligned.bioMod | 8 ++--- ...ion_musculoskeletal_dynamic_nmpc_cyclic.py | 31 ++++++++++++------- cocofest/models/dynamical_model.py | 7 +++++ 5 files changed, 54 insertions(+), 31 deletions(-) diff --git a/cocofest/dynamics/inverse_kinematics_and_dynamics.py b/cocofest/dynamics/inverse_kinematics_and_dynamics.py index 0c67d19..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]*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/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 index ccbb7b0..acbe888 100644 --- 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 @@ -1,9 +1,9 @@ """ 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. +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 @@ -47,12 +47,10 @@ control_type=ControlType.CONSTANT, ) -# sol = ocp.solve(solver=Solver.IPOPT(_max_iter=1)) sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) -SolutionToPickle(sol, "hand_cycling_external_forces.pkl", "").pickle() +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_forces.pkl").animate(model=biorbd_model) +PickleAnimate("hand_cycling_external_forces1.pkl").animate(model=biorbd_model) sol.animate(show_tracked_markers=True) -# sol.animate(viewer="pyorerun") sol.graphs(show_bounds=True) diff --git a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod index 6af3e32..e748d96 100644 --- a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod +++ b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod @@ -141,8 +141,8 @@ gravity 0 -9.8065999999999995 0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 rotations x - // ranges - // 0 2.2689280300000001 + ranges + -1 2.2689280300000001 endsegment // Segment @@ -211,7 +211,7 @@ gravity 0 -9.8065999999999995 0 //Pedal segment variables $wheel_radius 0.1 - $wheel_x_position 0.3 // X position of the wheel + $wheel_x_position 0.35 // X position of the wheel $wheel_y_position 0.0 // Y position of the wheel endvariables @@ -224,7 +224,7 @@ segment wheel_rotation 0.0 0.0 1.0 0.075 0.0 0.0 0.0 1.0 rotations z - rangesQ -2*pi 2*pi + rangesQ -4*pi 4*pi endsegment segment wheel 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..e3388ca 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 @@ -7,7 +7,7 @@ import os import biorbd -from bioptim import Solver +from bioptim import Solver, MultiCyclicNonlinearModelPredictiveControl, Solution from cocofest import ( DingModelPulseWidthFrequencyWithFatigue, NmpcFesMsk, @@ -22,16 +22,19 @@ muscles_model=[DingModelPulseWidthFrequencyWithFatigue(muscle_name="BIClong")], 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() +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, @@ -44,18 +47,22 @@ "with_residual_torque": True, }, ) -nmpc_fes_msk.n_cycles = 2 + +n_cycles = 5 + + +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": {}}, - get_all_iterations=True, + # get_all_iterations=True, ) 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() - -PickleAnimate(temp_pickle_file_path).animate(model=biorbd_model) - -os.remove(temp_pickle_file_path) +sol.print_cost() +sol.graphs(show_bounds=True) +sol.animate(n_frames=200, show_tracked_markers=True) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 66d133e..9f552be 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -33,6 +33,7 @@ def __init__( activate_residual_torque: bool = False, parameters: ParameterList = None, external_force_set: ExternalForceSetTimeSeries = None, + for_cycling: bool = False, ): """ The custom model that will be used in the optimal control program for the FES-MSK models @@ -70,6 +71,9 @@ def __init__( self.activate_force_length_relationship = activate_force_length_relationship self.activate_force_velocity_relationship = activate_force_velocity_relationship self.activate_residual_torque = activate_residual_torque + self.parameters_list = parameters + self.external_forces_set = external_force_set + self.for_cycling = for_cycling # ---- Absolutely needed methods ---- # def serialize(self) -> tuple[Callable, dict]: @@ -81,6 +85,9 @@ def serialize(self) -> tuple[Callable, dict]: "muscles_model": self.muscles_dynamics_model, "activate_force_length_relationship": self.activate_force_length_relationship, "activate_force_velocity_relationship": self.activate_force_velocity_relationship, + "activate_residual_torque": self.activate_residual_torque, + "parameters": self.parameters_list, + "external_force_set": self.external_forces_set, }, ) From d1f2422a5ea5234246318e96bac3b03379409342 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Wed, 4 Dec 2024 12:09:58 -0500 Subject: [PATCH 08/32] feat (nmpc with fes): additionnal files to work on examples --- ...le_fes_cycling_with_external_force_nmpc.py | 271 ++++++++++++ ..._Seth_pedal_aligned_test_one_marker.bioMod | 388 ++++++++++++++++++ ...ion_musculoskeletal_dynamic_nmpc_cyclic.py | 3 - cocofest/optimization/fes_ocp_dynamics.py | 137 ++++--- .../fes_ocp_dynamics_nmpc_cyclic.py | 99 ++++- 5 files changed, 842 insertions(+), 56 deletions(-) create mode 100644 cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py create mode 100644 cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod diff --git a/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py b/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py new file mode 100644 index 0000000..c8ccdcd --- /dev/null +++ b/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py @@ -0,0 +1,271 @@ +""" +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 numpy as np + +from bioptim import ( + Axis, + ConstraintFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + PhaseDynamics, + Solver, + MultiCyclicNonlinearModelPredictiveControl, + Solution, + SolutionMerge, + ControlType, +) + +from cocofest import get_circle_coord, OcpFesMsk, FesMskModel, DingModelPulseWidthFrequencyWithFatigue, OcpFes + + +class MyCyclicNMPC(MultiCyclicNonlinearModelPredictiveControl): + def advance_window_bounds_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_bounds_states(sol) # Allow the wheel to spin as much as needed + 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 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[-1, :] = q[-1, :] # Keep the previously found value for the wheel + return True + + +def prepare_nmpc( + model: FesMskModel, + stim_time: list, + pulse_width: dict, + cycle_duration: int | float, + n_cycles_to_advance: int, + n_cycles_simultaneous: int, + total_n_cycles: int, + objective: dict, +): + cycle_len = OcpFes.prepare_n_shooting(stim_time, cycle_duration) + total_n_shooting = cycle_len * n_cycles_simultaneous + + # --- EXTERNAL FORCES --- # + total_external_forces_frame = total_n_cycles * cycle_len if total_n_cycles >= n_cycles_simultaneous else total_n_shooting + external_force_set = ExternalForceSetTimeSeries(nb_frames=total_external_forces_frame) + external_force_array = np.array([0, 0, -1]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, total_external_forces_frame)) + external_force_set.add_torque(segment="wheel", values=reshape_values_array) + + # --- OBJECTIVE FUNCTION --- # + # 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"] + + from scipy.interpolate import interp1d + f = interp1d(np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), + np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), kind="linear") + x_new = f(np.linspace(0, -360 * n_cycles_simultaneous, total_n_shooting + 1)) + x_new_rad = np.deg2rad(x_new) + + circle_coord_list = np.array( + [ + get_circle_coord(theta, x_center, y_center, radius)[:-1] + for theta in x_new_rad + ] + ).T + + objective_functions = ObjectiveList() + objective_functions.add( + ObjectiveFcn.Mayer.TRACK_MARKERS, + weight=100000, + axes=[Axis.X, Axis.Y], + marker_index=0, + target=circle_coord_list, + node=Node.ALL, + phase=0, + quadratic=True, + ) + + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, quadratic=True) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=0.0001, quadratic=True) + + # --- DYNAMICS --- # + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + dynamics = DynamicsList() + dynamics.add( + model.declare_model_variables, + dynamic_function=model.muscle_dynamic, + expand_dynamics=True, + expand_continuity=False, + phase=0, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + numerical_data_timeseries=numerical_time_series, + with_contact=True, + ) + + # --- BOUNDS AND INITIAL GUESS --- # + # Path constraint: x_bounds, x_init + x_bounds, x_init = OcpFesMsk._set_bounds_fes(model) + x_bounds, x_init = OcpFesMsk._set_bounds_msk(x_bounds, x_init, model, msk_info={"bound_type": None}) + + q_guess, qdot_guess = OcpFesMsk._prepare_initial_guess_cycling(model.biorbd_path, + cycle_len, + x_center, + y_center, + radius, + n_cycles_simultaneous) + + x_initial_guess = {"q_guess": q_guess, "qdot_guess": qdot_guess} + x_bounds, x_init = OcpFesMsk._set_bounds_msk_for_cycling(x_bounds, x_init, model, x_initial_guess, + n_cycles_simultaneous) + + # Define control path constraint: u_bounds, u_init + u_bounds, u_init = OcpFesMsk._set_u_bounds_fes(model) + u_bounds, u_init = OcpFesMsk._set_u_bounds_msk(u_bounds, u_init, model, with_residual_torque=True) + u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, + interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) + + # --- CONSTRAINTS --- # + constraints = OcpFesMsk._build_constraints( + model, + cycle_len, + cycle_duration, + stim_time, + ControlType.CONSTANT, + custom_constraint=None, + external_forces=True, + simultaneous_cycle=n_cycles_simultaneous, + ) + + # constraints.add( + # ConstraintFcn.TRACK_MARKERS_VELOCITY, + # node=Node.START, + # marker_index=model.marker_index("wheel_center"), + # axes=[Axis.X, Axis.Y], + # ) + + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=Node.ALL, + axes=[Axis.X, Axis.Y], + ) + + # --- PARAMETERS --- # + (parameters, + parameters_bounds, + parameters_init, + parameter_objectives, + ) = OcpFesMsk._build_parameters( + model=model, + stim_time=stim_time, + pulse_event=None, + pulse_width=pulse_width, + pulse_intensity=None, + use_sx=True, + ) + + # rebuilding model for the OCP + model = FesMskModel( + name=model.name, + biorbd_path=model.biorbd_path, + muscles_model=model.muscles_dynamics_model, + 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, + for_cycling=True, + ) + + 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.RK4(), + control_type=ControlType.CONSTANT, + n_threads=8, + use_sx=True, + ) + + +def main(): + cycle_duration = 1 + n_cycles_to_advance = 1 + n_cycles_simultaneous = 2 + n_cycles = 2 + + model = FesMskModel( + name=None, + biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned.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 + + nmpc = 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], + pulse_width={ + "min": minimum_pulse_width, + "max": 0.0006, + "bimapping": False, + "same_for_all_muscles": False, + "fixed": False, + }, + cycle_duration=cycle_duration, + n_cycles_to_advance=n_cycles_to_advance, + n_cycles_simultaneous=n_cycles_simultaneous, + total_n_cycles=n_cycles, + objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}, + "minimize_residual_torque": True}, + ) + + def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): + return cycle_idx < n_cycles # True if there are still some cycle to perform + + # Solve the program + sol = nmpc.solve( + update_functions, + solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), + n_cycles_simultaneous=n_cycles_simultaneous, + ) + + sol.print_cost() + sol.graphs(show_bounds=True) + sol.animate(n_frames=200, show_tracked_markers=True) + + +if __name__ == "__main__": + main() diff --git a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod new file mode 100644 index 0000000..ce898a3 --- /dev/null +++ b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.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 -4*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 e3388ca..287129f 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 @@ -5,15 +5,12 @@ torque control. """ -import os import biorbd from bioptim import Solver, MultiCyclicNonlinearModelPredictiveControl, Solution from cocofest import ( DingModelPulseWidthFrequencyWithFatigue, NmpcFesMsk, FesMskModel, - SolutionToPickle, - PickleAnimate, ) model = FesMskModel( diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 2773ea9..0c73618 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -96,9 +96,15 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: 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, ) - numerical_time_series, with_contact, external_force_set = OcpFesMsk._prepare_numerical_time_series(input_dict["n_shooting"], input_dict["external_forces"]) + 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["n_shooting"] * input_dict["n_total_cycles"], input_dict["external_forces"], input_dict) + else: + numerical_time_series, with_contact, external_force_set = None, False, None + 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"]) @@ -129,6 +135,7 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: activate_residual_torque=input_dict["model"].activate_residual_torque, parameters=parameters, external_force_set=external_force_set, + for_cycling=input_dict["model"].for_cycling if "for_cycling" in input_dict["model"].__dict__.keys() else False, ) optimization_dict = { @@ -320,17 +327,49 @@ def prepare_ocp_for_cycling( def _prepare_optimization_problem_for_cycling(optimization_dict: dict, input_dict: dict) -> dict: OcpFesMsk._check_if_cycling_objectives_are_feasible(input_dict["objective"]["cycling"], input_dict["model"]) - q_guess, qdot_guess, u_guess = OcpFesMsk._prepare_initial_guess_cycling(input_dict["model"].biorbd_path, + if "cycling" in input_dict["objective"].keys(): + # Adding an objective function to track a marker in a circular trajectory + x_center = input_dict["objective"]["cycling"]["x_center"] + y_center = input_dict["objective"]["cycling"]["y_center"] + radius = input_dict["objective"]["cycling"]["radius"] + + from scipy.interpolate import interp1d + f = interp1d(np.linspace(0, -360 * input_dict["n_cycles_simultaneous"], 360 * input_dict["n_cycles_simultaneous"] + 1), + np.linspace(0, -360 * input_dict["n_cycles_simultaneous"], 360 * input_dict["n_cycles_simultaneous"] + 1), kind="linear") + x_new = f(np.linspace(0, -360 * input_dict["n_cycles_simultaneous"], input_dict["n_cycles_simultaneous"] * input_dict["n_shooting"] + 1)) + x_new_rad = np.deg2rad(x_new) + + circle_coord_list = np.array( + [ + get_circle_coord(theta, x_center, y_center, radius)[:-1] + for theta in x_new_rad + ] + ).T + + optimization_dict["objective_functions"].add( + ObjectiveFcn.Mayer.TRACK_MARKERS, + weight=100000, + axes=[Axis.X, Axis.Y], + marker_index=0, + target=circle_coord_list, + node=Node.ALL, + phase=0, + quadratic=True, + ) + + q_guess, qdot_guess = OcpFesMsk._prepare_initial_guess_cycling(input_dict["model"].biorbd_path, input_dict["n_shooting"], input_dict["objective"]["cycling"]["x_center"], input_dict["objective"]["cycling"]["y_center"], - input_dict["objective"]["cycling"]["radius"]) + input_dict["objective"]["cycling"]["radius"], + input_dict["n_cycles_simultaneous"],) + x_initial_guess = {"q_guess": q_guess, "qdot_guess": qdot_guess} - x_bounds, x_init = OcpFesMsk._set_bounds_msk_for_cycling(optimization_dict["x_bounds"], optimization_dict["x_init"], input_dict["model"], x_initial_guess) + x_bounds, x_init = OcpFesMsk._set_bounds_msk_for_cycling(optimization_dict["x_bounds"], optimization_dict["x_init"], input_dict["model"], x_initial_guess, input_dict["n_cycles_simultaneous"]) - u_initial_guess = {"u_guess": u_guess} - input_dict["msk_info"]["with_residual_torque"] = input_dict["msk_info"]["with_residual_torque"] if "with_residual_torque" in input_dict["msk_info"].keys() else False - u_bounds, u_init = OcpFesMsk._set_u_bounds_msk_for_cycling(optimization_dict["u_bounds"], optimization_dict["u_init"], input_dict["model"], input_dict["msk_info"]["with_residual_torque"], u_initial_guess) + u_bounds = BoundsList() + u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, + interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) if "with_contact" in input_dict["external_forces"] and input_dict["external_forces"]["with_contact"]: constraints = OcpFesMsk._build_constraints_for_cycling(optimization_dict["constraints"], input_dict["model"]) @@ -340,9 +379,10 @@ def _prepare_optimization_problem_for_cycling(optimization_dict: dict, input_dic optimization_dict_for_cycling = { "x_init": x_init, "x_bounds": x_bounds, - "u_init": u_init, + "u_init": optimization_dict["u_init"], "u_bounds": u_bounds, "constraints": constraints, + "objective_functions": optimization_dict["objective_functions"], } return optimization_dict_for_cycling @@ -396,20 +436,19 @@ def _fill_msk_dict(pulse_width, pulse_intensity, objective, msk_info): return pulse_width, pulse_intensity, objective, msk_info @staticmethod - def _prepare_numerical_time_series(n_shooting, external_forces): + def _prepare_numerical_time_series(n_shooting, external_forces, input_dict): - if external_forces: - external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) - reshape_values_array = np.tile(external_forces["torque"][:, np.newaxis], (1, n_shooting)) - external_force_set.add_torque(segment=external_forces["Segment_application"], - values=reshape_values_array) + 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) - numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} - with_contact = external_forces["with_contact"] if "with_contact" in external_forces.keys() else False - else: - numerical_time_series = None - with_contact = False - external_force_set = None + 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=external_forces["Segment_application"], + values=reshape_values_array) + + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + with_contact = external_forces["with_contact"] if "with_contact" in external_forces.keys() else False return numerical_time_series, with_contact, external_force_set @@ -582,7 +621,7 @@ def _build_parameters( ) @staticmethod - def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None): #, cycling=False): + def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None, simultaneous_cycle=1): #, cycling=False): constraints = ConstraintList() if model.activate_residual_torque and control_type == ControlType.LINEAR_CONTINUOUS: @@ -595,7 +634,7 @@ def _build_constraints(model, n_shooting, final_time, stim_time, control_type, c ) 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 + 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 @@ -640,30 +679,31 @@ def _build_constraints(model, n_shooting, final_time, stim_time, control_type, c @staticmethod def _build_constraints_for_cycling(constraints, model): - constraints.add( - ConstraintFcn.TRACK_MARKERS_VELOCITY, - node=Node.START, - marker_index=model.marker_index("wheel_center"), - axes=[Axis.X, Axis.Y], - ) + # constraints.add( + # ConstraintFcn.TRACK_MARKERS_VELOCITY, + # node=Node.START, + # marker_index=model.marker_index("wheel_center"), + # axes=[Axis.X, Axis.Y], + # ) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, first_marker="wheel_center", second_marker="global_wheel_center", - node=Node.START, + node=Node.ALL, axes=[Axis.X, Axis.Y], ) return constraints @staticmethod - def _prepare_initial_guess_cycling(biorbd_model_path, n_shooting, x_center, y_center, radius): - biorbd_model_path = "../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" #TODO : make it a def entry + def _prepare_initial_guess_cycling(biorbd_model_path, n_shooting, x_center, y_center, radius, n_cycles_simultaneous=1,): + biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" #TODO : make it a def entry q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( - biorbd_model_path, n_shooting, x_center, y_center, radius, ik_method="trf" + biorbd_model_path, n_shooting * n_cycles_simultaneous, x_center, y_center, radius, ik_method="trf", cycling_number=n_cycles_simultaneous ) - u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) - return q_guess, qdot_guess, u_guess + # u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) + + return q_guess, qdot_guess #, u_guess @staticmethod def _set_bounds_fes(bio_models): @@ -762,12 +802,15 @@ def _set_bounds_msk(x_bounds, x_init, bio_models, msk_info): return x_bounds, x_init @staticmethod - def _set_bounds_msk_for_cycling(x_bounds, x_init, bio_models, initial_guess): + def _set_bounds_msk_for_cycling(x_bounds, x_init, bio_models, initial_guess, n_cycles_simultaneous: int = 1): q_x_bounds = bio_models.bounds_from_ranges("q") qdot_x_bounds = bio_models.bounds_from_ranges("qdot") qdot_x_bounds.max[2] = [0, 0, 0] - x_bounds.add(key="q", bounds=q_x_bounds, phase=0) + x_bounds.add(key="q", bounds=q_x_bounds, phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) + x_bounds["q"].min[-1, :] = x_bounds["q"].min[-1, :] * n_cycles_simultaneous # Allow the wheel to spin as much as needed + x_bounds["q"].max[-1, :] = x_bounds["q"].max[-1, :] * n_cycles_simultaneous + x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) if initial_guess["q_guess"] is not None: @@ -1035,7 +1078,7 @@ def _sanity_check_msk_inputs( 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"] + cycling_objective_keys = ["x_center", "y_center", "radius"] 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}" @@ -1044,16 +1087,16 @@ def _sanity_check_msk_inputs( 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 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: diff --git a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py index e091b02..3406b2f 100644 --- a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py +++ b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py @@ -4,16 +4,31 @@ OdeSolver, MultiCyclicNonlinearModelPredictiveControl, ControlType, + SolutionMerge, + Solution, ) - +from .fes_ocp import OcpFes from .fes_ocp_dynamics import OcpFesMsk from ..models.dynamical_model import FesMskModel 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) + # 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 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 def update_stim(self, sol): stimulation_time = sol.decision_parameters()["pulse_apparition_time"] @@ -30,15 +45,17 @@ def update_stim(self, sol): def prepare_nmpc( model: FesMskModel = None, stim_time: list = None, - cycle_len: int = None, + # cycle_len: int = None, cycle_duration: int | float = None, n_cycles_simultaneous: int = None, n_cycles_to_advance: int = None, + n_total_cycles: int = None, pulse_event: dict = 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), @@ -49,15 +66,17 @@ def prepare_nmpc( input_dict = { "model": model, "stim_time": stim_time, - "n_shooting": cycle_len, + "n_shooting": OcpFes.prepare_n_shooting(stim_time, cycle_duration), "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_event": pulse_event, "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 +89,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 +106,71 @@ def prepare_nmpc( n_threads=optimization_dict["n_threads"], control_type=optimization_dict["control_type"], ) + + @staticmethod + def prepare_nmpc_for_cycling( + model: FesMskModel = None, + stim_time: list = None, + cycle_duration: int | float = None, + n_cycles_simultaneous: int = None, + n_cycles_to_advance: int = None, + n_total_cycles: int = None, + pulse_event: dict = 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, + "stim_time": stim_time, + "n_shooting": OcpFes.prepare_n_shooting(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_event": pulse_event, + "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"], + ) From 211544fb099810b7de5539a993f980eb881ef9a1 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Thu, 5 Dec 2024 18:26:42 -0500 Subject: [PATCH 09/32] feat (stimulation in model) : enabled the stimulation time in the model --- .../pulse_duration_optimization.py | 6 +- ...on_optimization_musculoskeletal_dynamic.py | 2 +- .../pulse_intensity_optimization.py | 3 +- cocofest/models/ding2003.py | 44 ++-------- cocofest/models/ding2003_with_fatigue.py | 15 ++-- cocofest/models/ding2007.py | 17 ++-- cocofest/models/ding2007_with_fatigue.py | 21 +++-- cocofest/models/dynamical_model.py | 9 +- cocofest/models/fes_model.py | 8 -- cocofest/models/hmed2018.py | 20 +++-- cocofest/models/hmed2018_with_fatigue.py | 20 +++-- cocofest/optimization/fes_ocp.py | 87 ++----------------- cocofest/optimization/fes_ocp_dynamics.py | 46 ++-------- 13 files changed, 88 insertions(+), 210 deletions(-) diff --git a/cocofest/examples/getting_started/pulse_duration_optimization.py b/cocofest/examples/getting_started/pulse_duration_optimization.py index 293c8bc..e764513 100644 --- a/cocofest/examples/getting_started/pulse_duration_optimization.py +++ b/cocofest/examples/getting_started/pulse_duration_optimization.py @@ -12,12 +12,14 @@ # 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 -model = ModelMaker.create_model("ding2007_with_fatigue", is_approximated=False) +model = ModelMaker.create_model("ding2007_with_fatigue", is_approximated=False, + stim_time=[0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45], + 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, pulse_width={ "min": minimum_pulse_width, 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_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/models/ding2003.py b/cocofest/models/ding2003.py index a09226f..52cea7e 100644 --- a/cocofest/models/ding2003.py +++ b/cocofest/models/ding2003.py @@ -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 + 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,7 +157,6 @@ def system_dynamics( cn: MX, f: MX, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, cn_sum: MX | float = None, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, @@ -169,8 +172,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) cn_sum: MX | float The sum of the ca_troponin_complex (unitless) force_length_relationship: MX | float @@ -182,6 +183,7 @@ def system_dynamics( ------- The value of the derivative of each state dx/dt at the current time t """ + t_stim_prev = self.all_stim cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev) f_dot = self.f_dot_fun( cn, @@ -367,7 +369,6 @@ def dynamics( cn=states[0], f=states[1], t=time, - t_stim_prev=stim_apparition, cn_sum=cn_sum, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, @@ -396,36 +397,3 @@ def declare_ding_variables( 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..011d8fd 100644 --- a/cocofest/models/ding2003_with_fatigue.py +++ b/cocofest/models/ding2003_with_fatigue.py @@ -31,12 +31,16 @@ 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 @@ -139,7 +143,6 @@ def system_dynamics( tau1: MX = None, km: MX = None, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, cn_sum: MX | float = None, force_length_relationship: MX | float = 1, force_velocity_relationship: MX | float = 1, @@ -161,8 +164,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) cn_sum: MX | float The sum of the ca_troponin_complex (unitless) force_length_relationship: MX | float @@ -174,6 +175,7 @@ def system_dynamics( ------- The value of the derivative of each state dx/dt at the current time t """ + t_stim_prev = self.all_stim cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev) f_dot = self.f_dot_fun( cn, @@ -278,13 +280,13 @@ def dynamics( """ model = fes_model if fes_model else nlp.model dxdt_fun = model.system_dynamics - stim_apparition = None + # stim_apparition = None cn_sum = None if model.is_approximated: cn_sum = controls[0] - else: - stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) + # else: + # stim_apparition = model.get_stim(nlp=nlp, parameters=parameters) return DynamicsEvaluation( dxdt=dxdt_fun( @@ -295,7 +297,6 @@ def dynamics( km=states[4], cn_sum=cn_sum, t=time, - t_stim_prev=stim_apparition, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, ), diff --git a/cocofest/models/ding2007.py b/cocofest/models/ding2007.py index 4adaf81..a58737a 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,6 +51,8 @@ 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, ) @@ -117,7 +121,6 @@ def system_dynamics( cn: MX, f: MX, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, pulse_width: MX = None, cn_sum: MX = None, a_scale: MX = None, @@ -135,8 +138,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_width: MX The pulsation duration of the current stimulation (s) cn_sum: MX | float @@ -152,6 +153,9 @@ def system_dynamics( ------- 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_width = self.previous_stim["pulse_width"] + pulse_width cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev) a_scale = ( a_scale @@ -321,15 +325,13 @@ def dynamics( 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) + if len(pulse_width) == 1 and len(nlp.model.stim_time) != 1: + pulse_width = pulse_width * len(nlp.model.stim_time) cn_sum = None a_scale = None @@ -338,7 +340,6 @@ def dynamics( cn=states[0], f=states[1], t=time, - t_stim_prev=stim_apparition, pulse_width=pulse_width, cn_sum=cn_sum, a_scale=a_scale, diff --git a/cocofest/models/ding2007_with_fatigue.py b/cocofest/models/ding2007_with_fatigue.py index 8dd6e2c..3ce7e44 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,9 +47,14 @@ 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, ) @@ -133,7 +140,6 @@ def system_dynamics( tau1: MX = None, km: MX = None, t: MX = None, - t_stim_prev: list[MX] | list[float] = None, pulse_width: MX = None, cn_sum: MX = None, a_scale: MX = None, @@ -157,8 +163,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_width: MX The time of the impulse (s) cn_sum: MX | float @@ -175,6 +179,9 @@ def system_dynamics( ------- 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_width = self.previous_stim["pulse_width"] + pulse_width cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev) a_scale = ( a_scale @@ -295,7 +302,6 @@ def dynamics( if model.is_approximated: cn_sum = controls[0] a_scale = controls[1] - stim_apparition = None pulse_width = None else: pulse_width = ( @@ -304,10 +310,8 @@ def dynamics( 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) + if len(pulse_width) == 1 and len(nlp.model.stim_time) != 1: + pulse_width = pulse_width * len(nlp.model.stim_time) cn_sum = None a_scale = None @@ -320,7 +324,6 @@ def dynamics( tau1=states[3], km=states[4], t=time, - t_stim_prev=stim_apparition, pulse_width=pulse_width, cn_sum=cn_sum, a_scale=a_scale, diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 9f552be..85cfd80 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -1,7 +1,7 @@ from typing import Callable import numpy as np -from casadi import vertcat, MX, SX, Function, horzcat +from casadi import vertcat, MX, SX, Function from bioptim import ( BiorbdModel, ExternalForceSetTimeSeries, @@ -28,6 +28,8 @@ 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_residual_torque: bool = False, @@ -66,6 +68,11 @@ 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 diff --git a/cocofest/models/fes_model.py b/cocofest/models/fes_model.py index b1ab740..87e96c3 100644 --- a/cocofest/models/fes_model.py +++ b/cocofest/models/fes_model.py @@ -209,11 +209,3 @@ def declare_ding_variables( """ - @abstractmethod - def set_pulse_apparition_time(self, value: list[MX]): - """ - - Returns - ------- - - """ diff --git a/cocofest/models/hmed2018.py b/cocofest/models/hmed2018.py index 3fb1d55..81fdd81 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,7 +122,6 @@ 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, @@ -132,8 +138,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 @@ -147,6 +151,9 @@ def system_dynamics( ------- 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, @@ -256,22 +263,19 @@ 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, diff --git a/cocofest/models/hmed2018_with_fatigue.py b/cocofest/models/hmed2018_with_fatigue.py index 321ee56..edb8d64 100644 --- a/cocofest/models/hmed2018_with_fatigue.py +++ b/cocofest/models/hmed2018_with_fatigue.py @@ -30,12 +30,19 @@ 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, ) @@ -124,7 +131,6 @@ 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, @@ -147,8 +153,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 @@ -162,6 +166,9 @@ def system_dynamics( ------- 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, @@ -269,15 +276,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,7 +292,6 @@ 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, diff --git a/cocofest/optimization/fes_ocp.py b/cocofest/optimization/fes_ocp.py index 39c4607..c3552dd 100644 --- a/cocofest/optimization/fes_ocp.py +++ b/cocofest/optimization/fes_ocp.py @@ -39,8 +39,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 +49,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,8 +59,6 @@ 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"], @@ -77,7 +73,7 @@ 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["model"].stim_time, input_dict["control_type"], ) u_bounds, u_init = OcpFes._set_u_bounds(input_dict["model"]) @@ -113,9 +109,7 @@ 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, @@ -131,12 +125,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 +152,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, @@ -220,16 +208,12 @@ def prepare_n_shooting(stim_time, final_time): 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 +228,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 +256,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 +291,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 +439,6 @@ def _build_fourier_coefficient(force_tracking): @staticmethod def _build_parameters( model, - stim_time, - pulse_event, pulse_width, pulse_intensity, use_sx, @@ -494,34 +448,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"]: diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 0c73618..513395f 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -41,8 +41,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"], @@ -59,7 +58,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, @@ -81,8 +79,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"], @@ -92,7 +88,6 @@ 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"], @@ -130,6 +125,8 @@ 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, @@ -164,9 +161,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, @@ -185,12 +180,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. @@ -225,10 +216,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, @@ -267,7 +256,6 @@ def prepare_ocp( @staticmethod def prepare_ocp_for_cycling( model: FesMskModel = None, - stim_time: list = None, final_time: int | float = None, pulse_event: dict = None, pulse_width: dict = None, @@ -283,10 +271,8 @@ def prepare_ocp_for_cycling( ): 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, @@ -470,8 +456,6 @@ def _declare_dynamics(bio_models, numerical_time_series, with_contact): @staticmethod def _build_parameters( model: FesMskModel, - stim_time: list, - pulse_event: dict, pulse_width: dict, pulse_intensity: dict, use_sx: bool = True, @@ -481,22 +465,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): @@ -621,8 +590,9 @@ def _build_parameters( ) @staticmethod - def _build_constraints(model, n_shooting, final_time, stim_time, control_type, custom_constraint=None, external_forces=None, simultaneous_cycle=1): #, cycling=False): + def _build_constraints(model, n_shooting, final_time, control_type, custom_constraint=None, external_forces=None, simultaneous_cycle=1): #, cycling=False): constraints = ConstraintList() + stim_time = model.muscles_dynamics_model[0].stim_time if model.activate_residual_torque and control_type == ControlType.LINEAR_CONTINUOUS: applied_node = n_shooting - 1 if external_forces else Node.END From 19b1cfee9fcfd00e609d70e0c47754207e05efd9 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Thu, 5 Dec 2024 18:35:06 -0500 Subject: [PATCH 10/32] feat (stimulation in model): now available for nmpc --- ...imization_musculoskeletal_dynamic_nmpc_cyclic.py | 2 +- cocofest/models/dynamical_model.py | 2 ++ .../optimization/fes_ocp_dynamics_nmpc_cyclic.py | 13 ++----------- 3 files changed, 5 insertions(+), 12 deletions(-) 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 287129f..0ab0039 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 @@ -17,6 +17,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, @@ -28,7 +29,6 @@ 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_duration=1, n_cycles_to_advance=1, n_cycles_simultaneous=3, diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 85cfd80..648903b 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -90,6 +90,8 @@ 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_residual_torque": self.activate_residual_torque, diff --git a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py index 3406b2f..af8c2a3 100644 --- a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py +++ b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py @@ -44,13 +44,10 @@ def update_stim(self, sol): @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, n_total_cycles: int = None, - pulse_event: dict = None, pulse_width: dict = None, pulse_intensity: dict = None, objective: dict = None, @@ -65,13 +62,11 @@ def prepare_nmpc( input_dict = { "model": model, - "stim_time": stim_time, - "n_shooting": OcpFes.prepare_n_shooting(stim_time, cycle_duration), + "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, "n_total_cycles": n_total_cycles, - "pulse_event": pulse_event, "pulse_width": pulse_width, "pulse_intensity": pulse_intensity, "objective": objective, @@ -110,12 +105,10 @@ def prepare_nmpc( @staticmethod def prepare_nmpc_for_cycling( model: FesMskModel = None, - stim_time: list = None, cycle_duration: int | float = None, n_cycles_simultaneous: int = None, n_cycles_to_advance: int = None, n_total_cycles: int = None, - pulse_event: dict = None, pulse_width: dict = None, pulse_intensity: dict = None, objective: dict = None, @@ -129,13 +122,11 @@ def prepare_nmpc_for_cycling( ): input_dict = { "model": model, - "stim_time": stim_time, - "n_shooting": OcpFes.prepare_n_shooting(stim_time, cycle_duration*n_cycles_simultaneous), + "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_event": pulse_event, "pulse_width": pulse_width, "pulse_intensity": pulse_intensity, "objective": objective, From 7ae30a56311a6ea89d2909c71adbfebafd06f904 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Thu, 5 Dec 2024 20:01:24 -0500 Subject: [PATCH 11/32] feat (stimulation in model) : update stimulation time and width in nmpc --- ...ion_musculoskeletal_dynamic_nmpc_cyclic.py | 73 ++++++++++++++++--- .../fes_ocp_dynamics_nmpc_cyclic.py | 20 ++--- 2 files changed, 72 insertions(+), 21 deletions(-) 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 0ab0039..c426bfd 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,9 +4,9 @@ The pulse width between minimal sensitivity threshold and 600us to satisfy the flexion and minimizing required elbow torque control. """ - +import numpy as np import biorbd -from bioptim import Solver, MultiCyclicNonlinearModelPredictiveControl, Solution +from bioptim import Solver, MultiCyclicNonlinearModelPredictiveControl, Solution, ObjectiveList, ObjectiveFcn, MultiCyclicCycleSolutions from cocofest import ( DingModelPulseWidthFrequencyWithFatigue, NmpcFesMsk, @@ -17,7 +17,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], + 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, @@ -26,6 +26,30 @@ minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 +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, @@ -37,15 +61,17 @@ "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, ) -n_cycles = 5 +n_cycles = 2 def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): @@ -55,11 +81,36 @@ def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_id sol = nmpc.solve( update_functions, solver=Solver.IPOPT(), + cycle_solutions=MultiCyclicCycleSolutions.ALL_CYCLES, + get_all_iterations=True, cyclic_options={"states": {}}, - # get_all_iterations=True, ) biorbd_model = biorbd.Model("../msk_models/arm26_biceps_1dof.bioMod") -sol.print_cost() -sol.graphs(show_bounds=True) -sol.animate(n_frames=200, show_tracked_markers=True) +# 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() + +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) + +# sol.graphs(show_bounds=True) +# sol.animate(n_frames=200, show_tracked_markers=True) diff --git a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py index af8c2a3..85e4945 100644 --- a/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py +++ b/cocofest/optimization/fes_ocp_dynamics_nmpc_cyclic.py @@ -10,12 +10,13 @@ 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, n_cycles_simultaneous=None, **extra): super(NmpcFesMsk, self).advance_window_bounds_states(sol) - # self.update_stim(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 @@ -31,15 +32,14 @@ def advance_window_initial_guess_states(self, sol, n_cycles_simultaneous=None): 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)) - - 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] - ) + # 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( From e02522a182a08406f7c0d053dee7a896821cc30c Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 17 Dec 2024 10:21:23 +0100 Subject: [PATCH 12/32] temp --- ...ycling_muscle_driven_and_external_force.py | 171 ++++++++++++ ...ycling_torque_driven_and_external_force.py | 181 ++++++++++++ ...cycling_with_external_force_nmpc_cyclic.py | 261 ++++++++++++++++++ ...le_fes_cycling_with_external_force_nmpc.py | 16 +- .../fes_cycling_with_external_force_nmpc.py | 105 +++++++ ...ion_musculoskeletal_dynamic_nmpc_cyclic.py | 32 +-- 6 files changed, 744 insertions(+), 22 deletions(-) create mode 100644 cocofest/examples/dynamics/cycling/cycling_muscle_driven_and_external_force.py create mode 100644 cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py create mode 100644 cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py create mode 100644 cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py diff --git a/cocofest/examples/dynamics/cycling/cycling_muscle_driven_and_external_force.py b/cocofest/examples/dynamics/cycling/cycling_muscle_driven_and_external_force.py new file mode 100644 index 0000000..af16843 --- /dev/null +++ b/cocofest/examples/dynamics/cycling/cycling_muscle_driven_and_external_force.py @@ -0,0 +1,171 @@ +""" +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 numpy as np + +from bioptim import ( + Axis, + BiorbdModel, + BoundsList, + ConstraintList, + ConstraintFcn, + CostType, + DynamicsFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InitialGuessList, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + PhaseDynamics, + Solver, + PhaseTransitionList, + PhaseTransitionFcn, +) + +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: + + # External forces + external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) + external_force_array = np.array([0, 0, -1]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, n_shooting)) + external_force_set.add_torque(segment="wheel", values=reshape_values_array) + + # Phase transitions not available with numerical time series + # phase_transitions = PhaseTransitionList() # TODO : transition phase cyclic + # phase_transitions.add(PhaseTransitionFcn.CYCLIC) + + # Dynamics + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + + bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) + + # 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=100000, + axes=[Axis.X, Axis.Y], + marker_index=0, + target=circle_coord_list, + node=Node.ALL_SHOOTING, + phase=0, + quadratic=True, + ) + + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=100, quadratic=True) + + # Dynamics + dynamics = DynamicsList() + dynamics.add( + DynamicsFcn.MUSCLE_DRIVEN, + expand_dynamics=True, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + numerical_data_timeseries=numerical_time_series, + with_contact=True, + ) + + # 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) + + # Modifying pedal speed bounds + qdot_x_bounds.max[2] = [0, 0, 0] + x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) + + # Define control path constraint + muscle_min, muscle_max, muscle_init = 0.0, 1.0, 0.5 + u_bounds = BoundsList() + u_bounds["muscles"] = [muscle_min] * bio_model.nb_muscles, [muscle_max] * bio_model.nb_muscles + u_init = InitialGuessList() + u_init["muscles"] = [muscle_init] * bio_model.nb_muscles + + # Initial q guess + x_init = InitialGuessList() + # # If warm start, the initial guess is the result of the inverse kinematics and dynamics + if initial_guess_warm_start: + biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" + 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) + + # Constraints + constraints = ConstraintList() + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=Node.START, + marker_index=bio_model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + ) + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=Node.START, + axes=[Axis.X, Axis.Y], + ) + + 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, + constraints=constraints, + # phase_transitions=phase_transitions, + ) + + +def main(): + # --- Prepare the ocp --- # + ocp = prepare_ocp( + biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned_test.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=False, _max_iter=10000))#, show_options=dict(show_bounds=True))) + sol.animate(viewer="pyorerun") + # sol.animate() + sol.graphs(show_bounds=True) + + +if __name__ == "__main__": + main() diff --git a/cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py b/cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py new file mode 100644 index 0000000..8ba5138 --- /dev/null +++ b/cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py @@ -0,0 +1,181 @@ +""" +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 numpy as np +from scipy.interpolate import interp1d + +from bioptim import ( + Axis, + BiorbdModel, + BoundsList, + ConstraintList, + ConstraintFcn, + CostType, + DynamicsFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InitialGuessList, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + PhaseDynamics, + Solver, + PhaseTransitionList, + PhaseTransitionFcn, + ControlType, +) + +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, + turn_number: int, + objective: dict, + initial_guess_warm_start: bool = False, +) -> OptimalControlProgram: + + # External forces + external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) + external_force_array = np.array([0, 0, -1]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, n_shooting)) + external_force_set.add_torque(segment="wheel", values=reshape_values_array) + + # Phase transitions not available with numerical time series + # phase_transitions = PhaseTransitionList() # TODO : transition phase cyclic + # phase_transitions.add(PhaseTransitionFcn.CYCLIC) + + # Dynamics + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) + + # 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"] + + f = interp1d(np.linspace(0, -360*turn_number, 360*turn_number+1), np.linspace(0, -360*turn_number, 360*turn_number+1), kind="linear") + x_new = f(np.linspace(0, -360*turn_number, n_shooting+1)) + x_new_rad = np.deg2rad(x_new) + + circle_coord_list = np.array( + [ + get_circle_coord(theta, x_center, y_center, radius)[:-1] + for theta in x_new_rad + ] + ).T + + objective_functions = ObjectiveList() + objective_functions.add( + ObjectiveFcn.Mayer.TRACK_MARKERS, + weight=100000, + axes=[Axis.X, Axis.Y], + marker_index=0, + target=circle_coord_list, + node=Node.ALL, + phase=0, + quadratic=True, + ) + + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, quadratic=True) + + # Dynamics + dynamics = DynamicsList() + dynamics.add( + DynamicsFcn.TORQUE_DRIVEN, + expand_dynamics=True, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + numerical_data_timeseries=numerical_time_series, + with_contact=True, + ) + + # 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["q"].min[-1, :] = x_bounds["q"].min[-1, :] * turn_number # Allow the wheel to spin as much as needed + x_bounds["q"].max[-1, :] = x_bounds["q"].max[-1, :] * turn_number + + # Modifying pedal speed bounds + qdot_x_bounds.max[2] = [0, 0, 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, -0]), max_bound=np.array([50, 50, 0]), 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: + biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" + q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( + biorbd_model_path, n_shooting, x_center, y_center, 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) + + constraints = ConstraintList() + # constraints.add( + # ConstraintFcn.TRACK_MARKERS_VELOCITY, + # node=Node.START, + # marker_index=bio_model.marker_index("wheel_center"), + # axes=[Axis.X, Axis.Y], + # ) + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=Node.ALL, + axes=[Axis.X, Axis.Y], + ) + + 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, + constraints=constraints, + ) + + +def main(): + # --- Prepare the ocp --- # + ocp = prepare_ocp( + biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod", + n_shooting=100, + final_time=5, + turn_number=5, + 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=False, _max_iter=10000)) #, show_options=dict(show_bounds=True)))#, show_options=dict(show_bounds=True))) + sol.animate(viewer="pyorerun", show_tracked_markers=True) + sol.graphs(show_bounds=True) + + +if __name__ == "__main__": + main() diff --git a/cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py b/cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py new file mode 100644 index 0000000..b9be157 --- /dev/null +++ b/cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py @@ -0,0 +1,261 @@ +""" +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 platform +import numpy as np + +from bioptim import ( + Axis, + BiorbdModel, + BoundsList, + ConstraintList, + ConstraintFcn, + CostType, + DynamicsFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InitialGuessList, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + PhaseDynamics, + Solver, + PhaseTransitionList, + PhaseTransitionFcn, + MultiCyclicNonlinearModelPredictiveControl, + Dynamics, + Objective, + Solution, + SolutionMerge, + MultiCyclicCycleSolutions, + ControlType, +) + +from cocofest import ( + get_circle_coord, + inverse_kinematics_cycling, + inverse_dynamics_cycling, +) + + +class MyCyclicNMPC(MultiCyclicNonlinearModelPredictiveControl): + def advance_window_bounds_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_bounds_states(sol) # Allow the wheel to spin as much as needed + 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 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[-1, :] = q[-1, :] # Keep the previously found value for the wheel + return True + + +def prepare_nmpc( + biorbd_model_path: str, + cycle_len: int, + cycle_duration: int | float, + n_cycles_to_advance: int, + n_cycles_simultaneous: int, + total_n_cycles: int, + objective: dict, + initial_guess_warm_start: bool = False, + dynamics_torque_driven: bool = True, + with_residual_torque: bool = False, + +): + if with_residual_torque and dynamics_torque_driven: + raise ValueError("Residual torque is only available for muscle driven dynamics") + + total_n_shooting = cycle_len * n_cycles_simultaneous + + # External forces + total_external_forces_frame = total_n_cycles * cycle_len if total_n_cycles >= n_cycles_simultaneous else total_n_shooting + external_force_set = ExternalForceSetTimeSeries(nb_frames=total_external_forces_frame) + external_force_array = np.array([0, 0, -1]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, total_external_forces_frame)) + external_force_set.add_torque(segment="wheel", values=reshape_values_array) + + # Dynamics + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) + bio_model.current_n_cycles = 1 + + # 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"] + + from scipy.interpolate import interp1d + f = interp1d(np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), + np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), kind="linear") + x_new = f(np.linspace(0, -360 * n_cycles_simultaneous, total_n_shooting + 1)) + x_new_rad = np.deg2rad(x_new) + + circle_coord_list = np.array( + [ + get_circle_coord(theta, x_center, y_center, radius)[:-1] + for theta in x_new_rad + ] + ).T + + objective_functions = ObjectiveList() + for i in [5, 10, 15, 20]: + objective_functions.add( + ObjectiveFcn.Mayer.TRACK_MARKERS, + weight=100000, + axes=[Axis.X, Axis.Y], + marker_index=0, + target=circle_coord_list[:, i], + node=i, + phase=0, + quadratic=True, + ) + + control_key = "tau" if dynamics_torque_driven else "muscles" + weight = 100 if dynamics_torque_driven else 1 + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key=control_key, weight=weight, quadratic=True) + if with_residual_torque: + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, quadratic=True) + # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=0.0001, quadratic=True) + + # Dynamics + chosen_dynamics = DynamicsFcn.TORQUE_DRIVEN if dynamics_torque_driven else DynamicsFcn.MUSCLE_DRIVEN + dynamics = DynamicsList() + dynamics.add( + chosen_dynamics, + expand_dynamics=True, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + numerical_data_timeseries=numerical_time_series, + with_contact=True, + # with_residual_torque=with_residual_torque, + ) + + # 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["q"].min[-1, :] = x_bounds["q"].min[-1, :] * n_cycles_simultaneous # Allow the wheel to spin as much as needed + x_bounds["q"].max[-1, :] = x_bounds["q"].max[-1, :] * n_cycles_simultaneous + + # Modifying pedal speed bounds + qdot_x_bounds.max[2] = [0, 0, 0] + x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,) + + # Define control path constraint + u_bounds = BoundsList() + if dynamics_torque_driven: + u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,) + else: + if with_residual_torque: + u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,) + muscle_min, muscle_max, muscle_init = 0.0, 1.0, 0.5 + u_bounds["muscles"] = [muscle_min] * bio_model.nb_muscles, [muscle_max] * bio_model.nb_muscles + u_init = InitialGuessList() + u_init["muscles"] = [muscle_init] * 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 and dynamics + if initial_guess_warm_start: + biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" + q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( + biorbd_model_path, total_n_shooting, x_center, y_center, radius, ik_method="trf", cycling_number=n_cycles_simultaneous + ) + x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) + x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) + # if not dynamics_torque_driven and with_residual_torque: + # u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) + # u_init.add("tau", u_guess, interpolation=InterpolationType.EACH_FRAME) + + # Constraints + constraints = ConstraintList() + + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=Node.START, + marker_index=bio_model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + ) + + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=Node.START, + axes=[Axis.X, Axis.Y], + ) + + return MyCyclicNMPC( + [bio_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, + ode_solver=OdeSolver.RK4(), + n_threads=8, + ) + + +def main(): + cycle_duration = 1 + cycle_len = 20 + n_cycles_to_advance = 1 + n_cycles_simultaneous = 2 + n_cycles = 2 + + nmpc = prepare_nmpc( + biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod", + cycle_len=cycle_len, + cycle_duration=cycle_duration, + n_cycles_to_advance=n_cycles_to_advance, + n_cycles_simultaneous=n_cycles_simultaneous, + total_n_cycles=n_cycles, + objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}}, + initial_guess_warm_start=True, + dynamics_torque_driven=True, + with_residual_torque=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 + + # Solve the program + sol = nmpc.solve( + update_functions, + solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), + n_cycles_simultaneous=n_cycles_simultaneous, + cyclic_options={"states": {}}, + cycle_solutions=MultiCyclicCycleSolutions.ALL_CYCLES, + get_all_iterations=True, + ) + + # sol.print_cost() + # sol.graphs(show_bounds=True) + sol[1][0].graphs(show_bounds=True) + sol[1][1].graphs(show_bounds=True) + + sol[0].graphs(show_bounds=True) + sol[0].animate(n_frames=100) + # sol.animate(n_frames=200, show_tracked_markers=True) + + +if __name__ == "__main__": + main() diff --git a/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py b/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py index c8ccdcd..72ece8f 100644 --- a/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py +++ b/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py @@ -52,7 +52,8 @@ def prepare_nmpc( total_n_cycles: int, objective: dict, ): - cycle_len = OcpFes.prepare_n_shooting(stim_time, cycle_duration) + # cycle_len = OcpFes.prepare_n_shooting(stim_time, cycle_duration) + cycle_len = 20 total_n_shooting = cycle_len * n_cycles_simultaneous # --- EXTERNAL FORCES --- # @@ -218,13 +219,13 @@ def main(): model = FesMskModel( name=None, - biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", + biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned_one_muscle.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), + # 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, @@ -237,6 +238,7 @@ def main(): nmpc = 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], + # stim_time=[0], pulse_width={ "min": minimum_pulse_width, "max": 0.0006, @@ -260,6 +262,8 @@ def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_id update_functions, solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), n_cycles_simultaneous=n_cycles_simultaneous, + # get_all_iterations=True, + cyclic_options={"states": {}}, ) sol.print_cost() diff --git a/cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py b/cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py new file mode 100644 index 0000000..fa736b6 --- /dev/null +++ b/cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py @@ -0,0 +1,105 @@ +""" +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 platform +import numpy as np + +from bioptim import ( + Axis, + BiorbdModel, + BoundsList, + ConstraintList, + ConstraintFcn, + CostType, + DynamicsFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InitialGuessList, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + PhaseDynamics, + Solver, + PhaseTransitionList, + PhaseTransitionFcn, + MultiCyclicNonlinearModelPredictiveControl, + Dynamics, + Objective, + Solution, + SolutionMerge, + MultiCyclicCycleSolutions, + ControlType, +) + +from cocofest import ( + get_circle_coord, + inverse_kinematics_cycling, + inverse_dynamics_cycling, +) + +from cocofest import NmpcFes, OcpFes, FesMskModel, DingModelPulseWidthFrequencyWithFatigue, SolutionToPickle, PickleAnimate, NmpcFesMsk + + +def main(): + 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 + n_cycles = 2 + nmpc_fes_msk = NmpcFesMsk + nmpc = nmpc_fes_msk.prepare_nmpc_for_cycling( + model=model, + stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], + cycle_duration=1, + n_cycles_to_advance=1, + n_cycles_simultaneous=3, + n_total_cycles=n_cycles, + pulse_width={ + "min": minimum_pulse_width, + "max": 0.0006, + "bimapping": False, + }, + objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}, + "minimize_residual_torque": True}, + msk_info={"with_residual_torque": True}, + external_forces={"torque": [0, 0, -1], "Segment_application": "wheel"} + ) + + 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( + update_functions, + solver=Solver.IPOPT(), + cyclic_options={"states": {}}, + ) + + sol.print_cost() + sol.graphs(show_bounds=True) + sol.animate(n_frames=200, show_tracked_markers=True) + + +if __name__ == "__main__": + main() + + + + 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 c426bfd..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 @@ -89,26 +89,26 @@ def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_id biorbd_model = biorbd.Model("../msk_models/arm26_biceps_1dof.bioMod") # sol.print_cost() -from matplotlib import pyplot as plt -from bioptim import SolutionMerge +# 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() +# 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() sol[1][0].graphs(show_bounds=True) sol[1][1].graphs(show_bounds=True) -# sol[1][0].animate(n_frames=100) +sol[1][0].animate(n_frames=100) sol[0].graphs(show_bounds=True) sol[0].animate(n_frames=100) From d2acab7f20306dbe6d1672f4ce71fd12cb92d3f3 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 7 Jan 2025 17:11:59 -0500 Subject: [PATCH 13/32] fix(integration): identified bug for integrators using rk4 --- .../examples/getting_started/pulse_duration_optimization.py | 3 ++- cocofest/models/ding2007.py | 2 +- cocofest/models/ding2007_with_fatigue.py | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/cocofest/examples/getting_started/pulse_duration_optimization.py b/cocofest/examples/getting_started/pulse_duration_optimization.py index e764513..8ef1501 100644 --- a/cocofest/examples/getting_started/pulse_duration_optimization.py +++ b/cocofest/examples/getting_started/pulse_duration_optimization.py @@ -3,7 +3,7 @@ This ocp was build to match a force value of 200N at the end of the last node. """ -from bioptim import Solver +from bioptim import Solver, OdeSolver from cocofest import OcpFes, ModelMaker # --- Build ocp --- # @@ -29,6 +29,7 @@ objective={"end_node_tracking": 100}, use_sx=True, n_threads=5, + ode_solver=OdeSolver.RK1(n_integration_steps=5), ) # --- Solve the program --- # diff --git a/cocofest/models/ding2007.py b/cocofest/models/ding2007.py index a58737a..59d9ae0 100644 --- a/cocofest/models/ding2007.py +++ b/cocofest/models/ding2007.py @@ -154,7 +154,7 @@ def system_dynamics( 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: + if self.all_stim != self.stim_time and not self.is_approximated: pulse_width = self.previous_stim["pulse_width"] + pulse_width cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev) a_scale = ( diff --git a/cocofest/models/ding2007_with_fatigue.py b/cocofest/models/ding2007_with_fatigue.py index 3ce7e44..6276d63 100644 --- a/cocofest/models/ding2007_with_fatigue.py +++ b/cocofest/models/ding2007_with_fatigue.py @@ -180,7 +180,7 @@ def system_dynamics( 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: + if self.all_stim != self.stim_time and not self.is_approximated: pulse_width = self.previous_stim["pulse_width"] + pulse_width cn_dot = self.calculate_cn_dot(cn, cn_sum, t, t_stim_prev) a_scale = ( From 248597f291a481585b16a26ecad93ff294aa31e0 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 14 Jan 2025 11:47:43 -0500 Subject: [PATCH 14/32] working on convergence --- .../dynamics/cycling/cycling_fes_driven.py | 349 ++++++++++++++++-- 1 file changed, 315 insertions(+), 34 deletions(-) diff --git a/cocofest/examples/dynamics/cycling/cycling_fes_driven.py b/cocofest/examples/dynamics/cycling/cycling_fes_driven.py index 107452e..daf31f9 100644 --- a/cocofest/examples/dynamics/cycling/cycling_fes_driven.py +++ b/cocofest/examples/dynamics/cycling/cycling_fes_driven.py @@ -5,67 +5,348 @@ reducing residual torque. """ -import numpy as np +# 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() + + +""" +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. +""" -from bioptim import CostType, Solver -import biorbd +import numpy as np -from cocofest import ( - DingModelPulseWidthFrequencyWithFatigue, - OcpFesMsk, - PlotCyclingResult, - SolutionToPickle, - FesMskModel, - PickleAnimate, +from bioptim import ( + Axis, + BiorbdModel, + ConstraintFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + PhaseDynamics, + Solver, + MultiCyclicNonlinearModelPredictiveControl, + MultiCyclicCycleSolutions, + Solution, + SolutionMerge, + ControlType, + OptimalControlProgram, ) +from cocofest import OcpFesMsk, FesMskModel, DingModelPulseWidthFrequency, OcpFes, CustomObjective + + +def prepare_ocp( + model: FesMskModel, + pulse_width: dict, + cycle_duration: int | float, + n_cycles_simultaneous: int, + objective: dict, +): + cycle_len = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration) + total_n_shooting = cycle_len * n_cycles_simultaneous + + # --- EXTERNAL FORCES --- # + total_external_forces_frame = total_n_shooting + external_force_set = ExternalForceSetTimeSeries(nb_frames=total_external_forces_frame) + external_force_array = np.array([0, 0, -1]) + reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, total_external_forces_frame)) + external_force_set.add_torque(segment="wheel", values=reshape_values_array) + + # --- DYNAMICS --- # + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + dynamics = DynamicsList() + dynamics.add( + model.declare_model_variables, + dynamic_function=model.muscle_dynamic, + expand_dynamics=True, + expand_continuity=False, + phase=0, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + numerical_data_timeseries=numerical_time_series, + with_contact=True, + ) + + # --- OBJECTIVE FUNCTION --- # + # 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"] + + objective_functions = ObjectiveList() + # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1000, quadratic=True) + # objective_functions.add(CustomObjective.minimize_overall_muscle_force_production, custom_type=ObjectiveFcn.Lagrange, weight=10, quadratic=True) + # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=0.0001, quadratic=True) + + # --- BOUNDS AND INITIAL GUESS --- # + # Path constraint: x_bounds, x_init + x_bounds, x_init = OcpFesMsk._set_bounds_fes(model) + q_guess, qdot_guess = OcpFesMsk._prepare_initial_guess_cycling(model.biorbd_path, + cycle_len, + x_center, + y_center, + radius, + n_cycles_simultaneous) + + # import matplotlib.pyplot as plt + # plt.plot(q_guess[0]) + # plt.plot(q_guess[1]) + # plt.show() + objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, key="q", index=0, target=q_guess[0][:-1], weight=1000, + quadratic=True) + objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, key="q", index=1, target=q_guess[1][:-1], weight=1000, + quadratic=True) + + + x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) + x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) + + q_x_bounds = model.bio_model.bounds_from_ranges("q") + q_x_bounds.min[0] = [-1] + q_x_bounds.max[0] = [2] + q_x_bounds.min[1] = [1] + q_x_bounds.min[2] = [q_x_bounds.min[2][0] * n_cycles_simultaneous] + q_x_bounds.max[2] = [5] + + # 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]] * (cycle_len * n_cycles_simultaneous + 1)) + # x_max_bound.append([q_x_bounds.max[i][0]] * (cycle_len * n_cycles_simultaneous + 1)) + # + # # # Resize bounds to reduce search space + # x_min_bound[0] = [-1] * (cycle_len * n_cycles_simultaneous + 1) + # x_max_bound[0] = [2] * (cycle_len * n_cycles_simultaneous + 1) + # x_min_bound[1] = [1] * (cycle_len * n_cycles_simultaneous + 1) + # x_max_bound[2] = [5] * (cycle_len * n_cycles_simultaneous + 1) + # x_min_bound[2] = [x_min_bound[2][0] * n_cycles_simultaneous] * (cycle_len * n_cycles_simultaneous + 1) # Allow the wheel to spin as much as needed + + # cardinal_node_list = [i * int(cycle_len / 4) for i in range(4 * n_cycles_simultaneous + 1)] + # for i in cardinal_node_list: + # x_min_bound[0][i] = x_init["q"].init[0][i] #- x_init["q"].init[0][i] * 0.01 + # x_max_bound[0][i] = x_init["q"].init[0][i] #+ #x_init["q"].init[0][i] * 0.01 + # x_min_bound[1][i] = x_init["q"].init[1][i] #- x_init["q"].init[1][i] * 0.01 + # x_max_bound[1][i] = x_init["q"].init[1][i] #+ x_init["q"].init[1][i] * 0.01 + + # x_bounds.add(key="q", min_bound=x_min_bound, max_bound=x_max_bound, phase=0, + # interpolation=InterpolationType.EACH_FRAME) + + x_bounds.add(key="q", bounds=q_x_bounds, phase=0) + + # Modifying pedal speed bounds + qdot_x_bounds = model.bio_model.bounds_from_ranges("qdot") + qdot_x_bounds.max[2] = [0, 0, 0] + x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) + + # Define control path constraint: u_bounds, u_init + u_bounds, u_init = OcpFesMsk._set_u_bounds_fes(model) + u_bounds, u_init = OcpFesMsk._set_u_bounds_msk(u_bounds, u_init, model, with_residual_torque=True) + u_bounds.add(key="tau", min_bound=np.array([-500, -500, -0]), max_bound=np.array([500, 500, 0]), phase=0, + interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) + + # --- CONSTRAINTS --- # + constraints = OcpFesMsk._build_constraints( + model, + cycle_len, + cycle_duration, + ControlType.CONSTANT, + custom_constraint=None, + external_forces=True, + simultaneous_cycle=n_cycles_simultaneous, + ) + + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=Node.ALL, + marker_index=model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + ) + + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + first_marker="wheel_center", + second_marker="global_wheel_center", + node=Node.START, + axes=[Axis.X, Axis.Y], + ) + + # cardinal_node_list = [i * int(cycle_len / 2) for i in range(2 * n_cycles_simultaneous + 1)] + # for i in cardinal_node_list: + # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=0, node=i, weight=10000000, target=q_guess[0][i], quadratic=True) + # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=1, node=i, weight=10000000, target=q_guess[1][i], quadratic=True) + # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=i, target=q_guess[0][i]) + # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=1, node=i, target=q_guess[1][i]) + # constraints.add(ConstraintFcn.TRACK_STATE, key="qdot", index=0, node=i, target=qdot_guess[0][i]) + # constraints.add(ConstraintFcn.TRACK_STATE, key="qdot", index=1, node=i, target=qdot_guess[1][i]) + + # --- PARAMETERS --- # + (parameters, + parameters_bounds, + parameters_init, + parameter_objectives, + ) = OcpFesMsk._build_parameters( + model=model, + pulse_width=pulse_width, + pulse_intensity=None, + use_sx=True, + ) + + # rebuilding model for the OCP + 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, + for_cycling=True, + ) + + return OptimalControlProgram( + [model], + dynamics, + n_shooting=total_n_shooting, + phase_time=3, + 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=3), + control_type=ControlType.CONSTANT, + n_threads=8, + use_sx=True, + ) + def main(): - minimum_pulse_width = DingModelPulseWidthFrequencyWithFatigue().pd0 + cycle_duration = 1 + n_cycles_simultaneous = 3 model = FesMskModel( name=None, - biorbd_path="../../msk_models/simplified_UL_Seth.bioMod", + biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned.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"), + DingModelPulseWidthFrequency(muscle_name="DeltoideusClavicle_A", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="DeltoideusScapula_P", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="TRIlong", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="BIC_long", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="BIC_brevis", is_approximated=False), ], + 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, # External forces will be added ) - ocp = OcpFesMsk.prepare_ocp( + minimum_pulse_width = DingModelPulseWidthFrequency().pd0 + + ocp = 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, + "same_for_all_muscles": False, + "fixed": 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, + cycle_duration=cycle_duration, + n_cycles_simultaneous=n_cycles_simultaneous, + objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}, + "minimize_residual_torque": True}, ) - 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) + # Solve the program + sol = ocp.solve( + solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), + ) - sol.graphs(show_bounds=False) - PlotCyclingResult(sol).plot(starting_location="E") + sol.graphs(show_bounds=True) + sol.animate(n_frames=200, show_tracked_markers=True) + print(sol.constraints) + print(sol.parameters) + print(sol.detailed_cost) if __name__ == "__main__": main() + From 205ab6603a72ae3b8b8716536e57679087fe4dd6 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 14 Jan 2025 12:30:51 -0500 Subject: [PATCH 15/32] . --- .../dynamics/cycling/cycling_torque_driven.py | 87 ++++++++++++++----- 1 file changed, 63 insertions(+), 24 deletions(-) diff --git a/cocofest/examples/dynamics/cycling/cycling_torque_driven.py b/cocofest/examples/dynamics/cycling/cycling_torque_driven.py index 55f4d8f..a312884 100644 --- a/cocofest/examples/dynamics/cycling/cycling_torque_driven.py +++ b/cocofest/examples/dynamics/cycling/cycling_torque_driven.py @@ -10,6 +10,8 @@ BiorbdModel, BoundsList, CostType, + ConstraintList, + ConstraintFcn, DynamicsFcn, DynamicsList, InitialGuessList, @@ -46,20 +48,24 @@ def prepare_ocp( 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 + # 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 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, - ) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", phase=0, node=Node.ALL_SHOOTING) + + # 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() @@ -78,20 +84,52 @@ def prepare_ocp( # 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) + u_bounds.add(key="tau", min_bound=np.array([-50, -50, 0]), max_bound=np.array([50, 50, 0]), 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) + + # The initial guess is the result of the inverse kinematics and dynamics + biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" + 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) + + # Constraints + constraints = ConstraintList() + cardinal_node_list = [i * int(n_shooting / 4) for i in range(4 + 1)] + for i in cardinal_node_list: + min_bound = x_init["q"].init[2][i]-x_init["q"].init[2][i]*0.01 + max_bound = x_init["q"].init[2][i]+x_init["q"].init[2][i]*0.01 + if min_bound > max_bound: + max_bound, min_bound = min_bound, max_bound + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, + key="q", + index=2, + target=x_init["q"].init[2][i], + # min_bound=[min_bound], + # max_bound=[max_bound], + node=i, + weight=100000, + ) + + constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, + node=Node.START, + first_marker="wheel_center", + second_marker="global_wheel_center", + axes=[Axis.X, Axis.Y],) + + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=Node.ALL_SHOOTING, + marker_index=bio_model.marker_index("wheel_center"), + axes=[Axis.X, Axis.Y], + ) return OptimalControlProgram( [bio_model], @@ -103,6 +141,7 @@ def prepare_ocp( x_init=x_init, u_init=u_init, objective_functions=objective_functions, + constraints=constraints, ode_solver=OdeSolver.RK4(), n_threads=8, ) @@ -111,7 +150,7 @@ def prepare_ocp( def main(): # --- Prepare the ocp --- # ocp = prepare_ocp( - biorbd_model_path="../../msk_models/simplified_UL_Seth.bioMod", + biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", n_shooting=100, final_time=1, objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}}, From 75d0f7d176bfd2a71e8d9e7ed0320191396c4934 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 17 Jan 2025 11:21:24 -0500 Subject: [PATCH 16/32] example regrouping torque/muscle/fes driven --- .../global_cycling_and_external_force.py | 353 ++++++++++++++++++ 1 file changed, 353 insertions(+) create mode 100644 cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py diff --git a/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py b/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py new file mode 100644 index 0000000..d288779 --- /dev/null +++ b/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py @@ -0,0 +1,353 @@ +""" +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 numpy as np +from scipy.interpolate import interp1d + +from bioptim import ( + Axis, + BiorbdModel, + BoundsList, + ConstraintList, + ConstraintFcn, + CostType, + DynamicsFcn, + DynamicsList, + ExternalForceSetTimeSeries, + InitialGuessList, + InterpolationType, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + PhaseDynamics, + Solver, + PhaseTransitionList, + PhaseTransitionFcn, + ControlType, +) + +from cocofest import ( + get_circle_coord, + inverse_kinematics_cycling, + inverse_dynamics_cycling, + FesMskModel, + CustomObjective, + DingModelPulseWidthFrequency, + OcpFesMsk, + OcpFes, +) + + +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, +) -> OptimalControlProgram: + + # Dynamics + numerical_time_series, external_force_set = set_external_forces(n_shooting, 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(n_shooting, pedal_config, turn_number) + + # Path constraint + x_bounds = set_bounds(model=model, + x_init=x_init, + n_shooting=n_shooting, + turn_number=turn_number, + interpolation_type=InterpolationType.EACH_FRAME, + cardinal=1) + # x_bounds = set_bounds(bio_model=bio_model, x_init=x_init, n_shooting=n_shooting, interpolation_type=InterpolationType.CONSTANT) + + # Control path constraint + u_bounds, u_init = set_u_bounds_and_init(model, dynamics_type_str=dynamics_type) + + # Constraints + constraints = set_constraints(model, 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 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=4), + 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, 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, + for_cycling=True, + ) + 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(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100000, quadratic=True) + # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_ACCELERATION, marker_index=model.marker_index("hand"), weight=100, quadratic=True) + # objective_functions.add(CustomObjective.minimize_overall_muscle_force_production, 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) + # if isinstance(model, BiorbdModel): + # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_ACCELERATION, + # marker_index=model.marker_index("hand"), weight=100, quadratic=True) + # else: # TO DO: check + # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_ACCELERATION, + # marker_index=model.marker_index("hand"), weight=100, quadratic=True) + + # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=1, 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_test_one_marker.bioMod" + # biorbd_model_path = "../../msk_models/arm26_cycling_pedal_aligned_contact_one_marker.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) + if dynamics_type_str == "fes_driven": + u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), 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 cardinal_node_list: + x_min_bound[2][i] = x_init["q"].init[2][i] - slack + x_max_bound[2][i] = x_init["q"].init[2][i] + 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[2] = [0, 0, 0] + # qdot_x_bounds.min[2] = [-60, -60, -60] + 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() + constraints.add( + ConstraintFcn.TRACK_MARKERS_VELOCITY, + node=Node.START, + marker_index=bio_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], + ) + + # cardinal_node_list = [i * int(300 / ((300 / 100) * 2)) for i in + # range(int((300 / 100) * 2 + 1))] + # for i in cardinal_node_list: + # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=i, target=x_init["q"].init[2][i]) + + return constraints + + +def main(): + # --- Prepare the ocp --- # + dynamics_type = "fes_driven" + model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod" + pulse_width = None + n_shooting = 300 + final_time = 1 + if dynamics_type == "torque_driven" or dynamics_type == "muscle_driven": + model = BiorbdModel(model_path) + elif dynamics_type == "fes_driven": + model = FesMskModel( + name=None, + biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", + muscles_model=[ + DingModelPulseWidthFrequency(muscle_name="DeltoideusClavicle_A", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="DeltoideusScapula_P", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="TRIlong", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="BIC_long", is_approximated=False), + DingModelPulseWidthFrequency(muscle_name="BIC_brevis", is_approximated=False), + ], + # 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], + # stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], + stim_time=list(np.round(np.linspace(0, 1, 34)[:-1], 3)), + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=True, + external_force_set=None, # External forces will be added + ) + pulse_width = {"min": DingModelPulseWidthFrequency().pd0, "max": 0.0006, "bimapping": False, "same_for_all_muscles": False, + "fixed": False} + n_shooting = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, final_time) + else: + raise ValueError("Dynamics type not recognized") + + ocp = prepare_ocp( + model=model, + n_shooting=n_shooting, + final_time=final_time, + turn_number=1, + pedal_config={"x_center": 0.35, "y_center": 0, "radius": 0.1}, + pulse_width=pulse_width, + dynamics_type=dynamics_type, + ) + ocp.add_plot_penalty(CostType.ALL) + sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) #, show_options=dict(show_bounds=True)))#, show_options=dict(show_bounds=True))) + sol.graphs(show_bounds=True) + sol.animate(viewer="pyorerun") + + # 914 iter before recuperation + + +if __name__ == "__main__": + main() From da857242454e4c27045561136e04ffa2faf96af7 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 17 Jan 2025 14:40:57 -0500 Subject: [PATCH 17/32] correct pedal inertial for model --- .../simplified_UL_Seth_pedal_aligned.bioMod | 11 ++++++----- ...ified_UL_Seth_pedal_aligned_test_one_marker.bioMod | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod index e748d96..2ce821a 100644 --- a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod +++ b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned.bioMod @@ -224,17 +224,18 @@ segment wheel_rotation 0.0 0.0 1.0 0.075 0.0 0.0 0.0 1.0 rotations z - rangesQ -4*pi 4*pi + rangesQ -12*pi 4*pi endsegment segment wheel parent wheel_rotation RT 0 0 0 xyz $wheel_radius 0 0 - mass 1 + mass 0.5 inertia - 1 0 0 - 0 1 0 - 0 0 1 + 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 diff --git a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod index ce898a3..f907b34 100644 --- a/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod +++ b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod @@ -224,7 +224,7 @@ segment wheel_rotation 0.0 0.0 1.0 0.075 0.0 0.0 0.0 1.0 rotations z - rangesQ -4*pi 4*pi + rangesQ -12*pi 4*pi endsegment segment wheel From 7f298b0279c1661b76f861999047a2b45bfcc149 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 17 Jan 2025 14:41:23 -0500 Subject: [PATCH 18/32] Making truncation possible again --- cocofest/models/ding2003.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cocofest/models/ding2003.py b/cocofest/models/ding2003.py index 52cea7e..7252295 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, @@ -247,7 +247,12 @@ def cn_sum_fun(self, r0: MX | float, t: MX, t_stim_prev: list[MX], lambda_i: lis 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) + if i > self._sum_stim_truncation and isinstance(self._sum_stim_truncation, int): + coefficient = 1 if self.is_approximated else if_else(logic_and(t_stim_prev[i] <= t, + t - t_stim_prev[i] <= self._sum_stim_truncation * (t_stim_prev[i]-t_stim_prev[i-1])), + 1, 0) + else: + 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 return sum_multiplier From 227b327b9f0e963bfc585f951e147fe6987d6abb Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 17 Jan 2025 14:42:02 -0500 Subject: [PATCH 19/32] Reducing node shooting to increase optimization time --- .../dynamics/cycling/global_cycling_and_external_force.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py b/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py index d288779..51bb7bb 100644 --- a/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py +++ b/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py @@ -108,7 +108,7 @@ def prepare_ocp( x_init=x_init, u_init=u_init, objective_functions=objective_functions, - ode_solver=OdeSolver.RK1(n_integration_steps=4), + ode_solver=OdeSolver.RK1(n_integration_steps=20), n_threads=20, constraints=constraints, parameters=parameters, @@ -320,7 +320,7 @@ def main(): # 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], # stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], - stim_time=list(np.round(np.linspace(0, 1, 34)[:-1], 3)), + stim_time=list(np.linspace(0, 1, 34)[:-1]), activate_force_length_relationship=True, activate_force_velocity_relationship=True, activate_residual_torque=True, @@ -328,7 +328,8 @@ def main(): ) pulse_width = {"min": DingModelPulseWidthFrequency().pd0, "max": 0.0006, "bimapping": False, "same_for_all_muscles": False, "fixed": False} - n_shooting = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, final_time) + # n_shooting = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, final_time) + n_shooting = 33 else: raise ValueError("Dynamics type not recognized") From 005f439305c1838b489c9677b0edd8ae8c5e94d7 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 31 Jan 2025 14:19:15 -0500 Subject: [PATCH 20/32] FIX: now all parameters sent are associated to the good muscle --- cocofest/models/dynamical_model.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 648903b..312480b 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -231,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) @@ -265,7 +269,7 @@ def muscles_joint_torque( time, muscle_states, controls, - parameters, + muscle_parameters, algebraic_states, numerical_data_timeseries, nlp, From f065f85469aaf4eb3ab420ba0b8222ef57718d3d Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 31 Jan 2025 14:41:40 -0500 Subject: [PATCH 21/32] example cleaning --- .../dynamics/cycling/cycling_muscle_driven.py | 124 ------------ ...ycling_muscle_driven_and_external_force.py | 171 ----------------- .../dynamics/cycling/cycling_torque_driven.py | 166 ---------------- ...ycling_torque_driven_and_external_force.py | 181 ------------------ ... cycling_with_different_driven_methods.py} | 8 +- 5 files changed, 3 insertions(+), 647 deletions(-) delete mode 100644 cocofest/examples/dynamics/cycling/cycling_muscle_driven.py delete mode 100644 cocofest/examples/dynamics/cycling/cycling_muscle_driven_and_external_force.py delete mode 100644 cocofest/examples/dynamics/cycling/cycling_torque_driven.py delete mode 100644 cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py rename cocofest/examples/dynamics/cycling/{global_cycling_and_external_force.py => cycling_with_different_driven_methods.py} (99%) 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_muscle_driven_and_external_force.py b/cocofest/examples/dynamics/cycling/cycling_muscle_driven_and_external_force.py deleted file mode 100644 index af16843..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_muscle_driven_and_external_force.py +++ /dev/null @@ -1,171 +0,0 @@ -""" -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 numpy as np - -from bioptim import ( - Axis, - BiorbdModel, - BoundsList, - ConstraintList, - ConstraintFcn, - CostType, - DynamicsFcn, - DynamicsList, - ExternalForceSetTimeSeries, - InitialGuessList, - InterpolationType, - Node, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - OptimalControlProgram, - PhaseDynamics, - Solver, - PhaseTransitionList, - PhaseTransitionFcn, -) - -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: - - # External forces - external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) - external_force_array = np.array([0, 0, -1]) - reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, n_shooting)) - external_force_set.add_torque(segment="wheel", values=reshape_values_array) - - # Phase transitions not available with numerical time series - # phase_transitions = PhaseTransitionList() # TODO : transition phase cyclic - # phase_transitions.add(PhaseTransitionFcn.CYCLIC) - - # Dynamics - numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} - - bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) - - # 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=100000, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list, - node=Node.ALL_SHOOTING, - phase=0, - quadratic=True, - ) - - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=100, quadratic=True) - - # Dynamics - dynamics = DynamicsList() - dynamics.add( - DynamicsFcn.MUSCLE_DRIVEN, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - with_contact=True, - ) - - # 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) - - # Modifying pedal speed bounds - qdot_x_bounds.max[2] = [0, 0, 0] - x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) - - # Define control path constraint - muscle_min, muscle_max, muscle_init = 0.0, 1.0, 0.5 - u_bounds = BoundsList() - u_bounds["muscles"] = [muscle_min] * bio_model.nb_muscles, [muscle_max] * bio_model.nb_muscles - u_init = InitialGuessList() - u_init["muscles"] = [muscle_init] * bio_model.nb_muscles - - # Initial q guess - x_init = InitialGuessList() - # # If warm start, the initial guess is the result of the inverse kinematics and dynamics - if initial_guess_warm_start: - biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" - 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) - - # Constraints - constraints = ConstraintList() - constraints.add( - ConstraintFcn.TRACK_MARKERS_VELOCITY, - node=Node.START, - marker_index=bio_model.marker_index("wheel_center"), - axes=[Axis.X, Axis.Y], - ) - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - first_marker="wheel_center", - second_marker="global_wheel_center", - node=Node.START, - axes=[Axis.X, Axis.Y], - ) - - 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, - constraints=constraints, - # phase_transitions=phase_transitions, - ) - - -def main(): - # --- Prepare the ocp --- # - ocp = prepare_ocp( - biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned_test.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=False, _max_iter=10000))#, show_options=dict(show_bounds=True))) - sol.animate(viewer="pyorerun") - # sol.animate() - sol.graphs(show_bounds=True) - - -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 a312884..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_torque_driven.py +++ /dev/null @@ -1,166 +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, - ConstraintList, - ConstraintFcn, - 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 - objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", phase=0, node=Node.ALL_SHOOTING) - - # 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, 0]), max_bound=np.array([50, 50, 0]), phase=0) - - # Initial q guess - x_init = InitialGuessList() - u_init = InitialGuessList() - - # The initial guess is the result of the inverse kinematics and dynamics - biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" - 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) - - # Constraints - constraints = ConstraintList() - cardinal_node_list = [i * int(n_shooting / 4) for i in range(4 + 1)] - for i in cardinal_node_list: - min_bound = x_init["q"].init[2][i]-x_init["q"].init[2][i]*0.01 - max_bound = x_init["q"].init[2][i]+x_init["q"].init[2][i]*0.01 - if min_bound > max_bound: - max_bound, min_bound = min_bound, max_bound - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, - key="q", - index=2, - target=x_init["q"].init[2][i], - # min_bound=[min_bound], - # max_bound=[max_bound], - node=i, - weight=100000, - ) - - constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, - node=Node.START, - first_marker="wheel_center", - second_marker="global_wheel_center", - axes=[Axis.X, Axis.Y],) - - constraints.add( - ConstraintFcn.TRACK_MARKERS_VELOCITY, - node=Node.ALL_SHOOTING, - marker_index=bio_model.marker_index("wheel_center"), - axes=[Axis.X, Axis.Y], - ) - - 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, - constraints=constraints, - ode_solver=OdeSolver.RK4(), - n_threads=8, - ) - - -def main(): - # --- Prepare the ocp --- # - ocp = prepare_ocp( - biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned.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(viewer="pyorerun") - sol.graphs() - - -if __name__ == "__main__": - main() diff --git a/cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py b/cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py deleted file mode 100644 index 8ba5138..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_torque_driven_and_external_force.py +++ /dev/null @@ -1,181 +0,0 @@ -""" -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 numpy as np -from scipy.interpolate import interp1d - -from bioptim import ( - Axis, - BiorbdModel, - BoundsList, - ConstraintList, - ConstraintFcn, - CostType, - DynamicsFcn, - DynamicsList, - ExternalForceSetTimeSeries, - InitialGuessList, - InterpolationType, - Node, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - OptimalControlProgram, - PhaseDynamics, - Solver, - PhaseTransitionList, - PhaseTransitionFcn, - ControlType, -) - -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, - turn_number: int, - objective: dict, - initial_guess_warm_start: bool = False, -) -> OptimalControlProgram: - - # External forces - external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) - external_force_array = np.array([0, 0, -1]) - reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, n_shooting)) - external_force_set.add_torque(segment="wheel", values=reshape_values_array) - - # Phase transitions not available with numerical time series - # phase_transitions = PhaseTransitionList() # TODO : transition phase cyclic - # phase_transitions.add(PhaseTransitionFcn.CYCLIC) - - # Dynamics - numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} - bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) - - # 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"] - - f = interp1d(np.linspace(0, -360*turn_number, 360*turn_number+1), np.linspace(0, -360*turn_number, 360*turn_number+1), kind="linear") - x_new = f(np.linspace(0, -360*turn_number, n_shooting+1)) - x_new_rad = np.deg2rad(x_new) - - circle_coord_list = np.array( - [ - get_circle_coord(theta, x_center, y_center, radius)[:-1] - for theta in x_new_rad - ] - ).T - - objective_functions = ObjectiveList() - objective_functions.add( - ObjectiveFcn.Mayer.TRACK_MARKERS, - weight=100000, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list, - node=Node.ALL, - phase=0, - quadratic=True, - ) - - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, quadratic=True) - - # Dynamics - dynamics = DynamicsList() - dynamics.add( - DynamicsFcn.TORQUE_DRIVEN, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - with_contact=True, - ) - - # 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["q"].min[-1, :] = x_bounds["q"].min[-1, :] * turn_number # Allow the wheel to spin as much as needed - x_bounds["q"].max[-1, :] = x_bounds["q"].max[-1, :] * turn_number - - # Modifying pedal speed bounds - qdot_x_bounds.max[2] = [0, 0, 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, -0]), max_bound=np.array([50, 50, 0]), 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: - biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" - q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( - biorbd_model_path, n_shooting, x_center, y_center, 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) - - constraints = ConstraintList() - # constraints.add( - # ConstraintFcn.TRACK_MARKERS_VELOCITY, - # node=Node.START, - # marker_index=bio_model.marker_index("wheel_center"), - # axes=[Axis.X, Axis.Y], - # ) - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - first_marker="wheel_center", - second_marker="global_wheel_center", - node=Node.ALL, - axes=[Axis.X, Axis.Y], - ) - - 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, - constraints=constraints, - ) - - -def main(): - # --- Prepare the ocp --- # - ocp = prepare_ocp( - biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod", - n_shooting=100, - final_time=5, - turn_number=5, - 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=False, _max_iter=10000)) #, show_options=dict(show_bounds=True)))#, show_options=dict(show_bounds=True))) - sol.animate(viewer="pyorerun", show_tracked_markers=True) - sol.graphs(show_bounds=True) - - -if __name__ == "__main__": - main() diff --git a/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py similarity index 99% rename from cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py rename to cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py index 51bb7bb..3b8075e 100644 --- a/cocofest/examples/dynamics/cycling/global_cycling_and_external_force.py +++ b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py @@ -69,7 +69,7 @@ def prepare_ocp( n_shooting=n_shooting, turn_number=turn_number, interpolation_type=InterpolationType.EACH_FRAME, - cardinal=1) + cardinal=4) # x_bounds = set_bounds(bio_model=bio_model, x_init=x_init, n_shooting=n_shooting, interpolation_type=InterpolationType.CONSTANT) # Control path constraint @@ -262,7 +262,7 @@ def set_bounds(model, x_init, n_shooting, turn_number, interpolation_type=Interp x_bounds.add(key="q", bounds=q_x_bounds, phase=0) # Modifying pedal speed bounds - # qdot_x_bounds.max[2] = [0, 0, 0] + qdot_x_bounds.max[2] = [0, 0, 0] # qdot_x_bounds.min[2] = [-60, -60, -60] x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) return x_bounds @@ -301,7 +301,7 @@ def main(): dynamics_type = "fes_driven" model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod" pulse_width = None - n_shooting = 300 + n_shooting = 100 final_time = 1 if dynamics_type == "torque_driven" or dynamics_type == "muscle_driven": model = BiorbdModel(model_path) @@ -347,8 +347,6 @@ def main(): sol.graphs(show_bounds=True) sol.animate(viewer="pyorerun") - # 914 iter before recuperation - if __name__ == "__main__": main() From 62974a55e7f55d2e03372960aec950566a6f5d7f Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 31 Jan 2025 14:42:03 -0500 Subject: [PATCH 22/32] truncation adjustment --- cocofest/models/ding2003.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cocofest/models/ding2003.py b/cocofest/models/ding2003.py index 7252295..2225d78 100644 --- a/cocofest/models/ding2003.py +++ b/cocofest/models/ding2003.py @@ -247,7 +247,7 @@ def cn_sum_fun(self, r0: MX | float, t: MX, t_stim_prev: list[MX], lambda_i: lis 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 - if i > self._sum_stim_truncation and isinstance(self._sum_stim_truncation, int): + if isinstance(self._sum_stim_truncation, int) and i > self._sum_stim_truncation: coefficient = 1 if self.is_approximated else if_else(logic_and(t_stim_prev[i] <= t, t - t_stim_prev[i] <= self._sum_stim_truncation * (t_stim_prev[i]-t_stim_prev[i-1])), 1, 0) From deba9361a993db454b032435638acd591583273b Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 31 Jan 2025 14:53:06 -0500 Subject: [PATCH 23/32] fes_ocp cleaning --- cocofest/optimization/fes_ocp.py | 25 +- cocofest/optimization/fes_ocp_dynamics.py | 267 +--------------------- 2 files changed, 22 insertions(+), 270 deletions(-) diff --git a/cocofest/optimization/fes_ocp.py b/cocofest/optimization/fes_ocp.py index c3552dd..2786fe6 100644 --- a/cocofest/optimization/fes_ocp.py +++ b/cocofest/optimization/fes_ocp.py @@ -114,7 +114,7 @@ def prepare_ocp( 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, ): @@ -196,15 +196,20 @@ 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) + step = round(stim_time[1] - stim_time[0], 4) # Calculate the initial step size + if all(round(stim_time[i + 1] - stim_time[i], 4) == step for i in range(len(stim_time) - 1)): + n_shooting = len(stim_time) + + else: + 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) return n_shooting @staticmethod diff --git a/cocofest/optimization/fes_ocp_dynamics.py b/cocofest/optimization/fes_ocp_dynamics.py index 513395f..8218de5 100644 --- a/cocofest/optimization/fes_ocp_dynamics.py +++ b/cocofest/optimization/fes_ocp_dynamics.py @@ -1,7 +1,6 @@ import numpy as np from bioptim import ( - Axis, BoundsList, ConstraintList, ControlType, @@ -23,18 +22,12 @@ ) from ..custom_objectives import CustomObjective -from ..dynamics.inverse_kinematics_and_dynamics import get_circle_coord -from ..models.ding2003 import DingModelFrequency from ..models.ding2007 import DingModelPulseWidthFrequency from ..models.dynamical_model import FesMskModel from ..models.hmed2018 import DingModelPulseIntensityFrequency from ..optimization.fes_ocp import OcpFes from ..fourier_approx import FourierSeries from ..custom_constraints import CustomConstraint -from ..dynamics.inverse_kinematics_and_dynamics import ( - inverse_kinematics_cycling, - inverse_dynamics_cycling, -) class OcpFesMsk: @@ -96,7 +89,7 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: 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["n_shooting"] * input_dict["n_total_cycles"], input_dict["external_forces"], input_dict) + 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 @@ -132,7 +125,6 @@ def _prepare_optimization_problem(input_dict: dict) -> dict: activate_residual_torque=input_dict["model"].activate_residual_torque, parameters=parameters, external_force_set=external_force_set, - for_cycling=input_dict["model"].for_cycling if "for_cycling" in input_dict["model"].__dict__.keys() else False, ) optimization_dict = { @@ -253,126 +245,6 @@ def prepare_ocp( n_threads=optimization_dict["n_threads"], ) - @staticmethod - def prepare_ocp_for_cycling( - model: FesMskModel = None, - final_time: int | float = None, - pulse_event: dict = None, - pulse_width: dict = None, - pulse_intensity: dict = None, - objective: dict = None, - msk_info: dict = None, - use_sx: bool = True, - initial_guess_warm_start: bool = False, - ode_solver: OdeSolverBase = OdeSolver.RK4(n_integration_steps=1), - control_type: ControlType = ControlType.CONSTANT, - n_threads: int = 1, - external_forces: dict = None, - ): - input_dict = { - "model": model, - "n_shooting": OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, final_time), - "final_time": final_time, - "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 OptimalControlProgram( - bio_model=[optimization_dict["model"]], - dynamics=optimization_dict["dynamics"], - n_shooting=optimization_dict["n_shooting"], - phase_time=optimization_dict["final_time"], - 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"], - ) - - @staticmethod - def _prepare_optimization_problem_for_cycling(optimization_dict: dict, input_dict: dict) -> dict: - OcpFesMsk._check_if_cycling_objectives_are_feasible(input_dict["objective"]["cycling"], input_dict["model"]) - - if "cycling" in input_dict["objective"].keys(): - # Adding an objective function to track a marker in a circular trajectory - x_center = input_dict["objective"]["cycling"]["x_center"] - y_center = input_dict["objective"]["cycling"]["y_center"] - radius = input_dict["objective"]["cycling"]["radius"] - - from scipy.interpolate import interp1d - f = interp1d(np.linspace(0, -360 * input_dict["n_cycles_simultaneous"], 360 * input_dict["n_cycles_simultaneous"] + 1), - np.linspace(0, -360 * input_dict["n_cycles_simultaneous"], 360 * input_dict["n_cycles_simultaneous"] + 1), kind="linear") - x_new = f(np.linspace(0, -360 * input_dict["n_cycles_simultaneous"], input_dict["n_cycles_simultaneous"] * input_dict["n_shooting"] + 1)) - x_new_rad = np.deg2rad(x_new) - - circle_coord_list = np.array( - [ - get_circle_coord(theta, x_center, y_center, radius)[:-1] - for theta in x_new_rad - ] - ).T - - optimization_dict["objective_functions"].add( - ObjectiveFcn.Mayer.TRACK_MARKERS, - weight=100000, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list, - node=Node.ALL, - phase=0, - quadratic=True, - ) - - q_guess, qdot_guess = OcpFesMsk._prepare_initial_guess_cycling(input_dict["model"].biorbd_path, - input_dict["n_shooting"], - input_dict["objective"]["cycling"]["x_center"], - input_dict["objective"]["cycling"]["y_center"], - input_dict["objective"]["cycling"]["radius"], - input_dict["n_cycles_simultaneous"],) - - x_initial_guess = {"q_guess": q_guess, "qdot_guess": qdot_guess} - x_bounds, x_init = OcpFesMsk._set_bounds_msk_for_cycling(optimization_dict["x_bounds"], optimization_dict["x_init"], input_dict["model"], x_initial_guess, input_dict["n_cycles_simultaneous"]) - - u_bounds = BoundsList() - u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, - interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) - - if "with_contact" in input_dict["external_forces"] and input_dict["external_forces"]["with_contact"]: - constraints = OcpFesMsk._build_constraints_for_cycling(optimization_dict["constraints"], input_dict["model"]) - else: - constraints = optimization_dict["constraints"] - - optimization_dict_for_cycling = { - "x_init": x_init, - "x_bounds": x_bounds, - "u_init": optimization_dict["u_init"], - "u_bounds": u_bounds, - "constraints": constraints, - "objective_functions": optimization_dict["objective_functions"], - } - - return optimization_dict_for_cycling - @staticmethod def _fill_msk_dict(pulse_width, pulse_intensity, objective, msk_info): @@ -398,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, @@ -422,7 +293,7 @@ def _fill_msk_dict(pulse_width, pulse_intensity, objective, msk_info): return pulse_width, pulse_intensity, objective, msk_info @staticmethod - def _prepare_numerical_time_series(n_shooting, external_forces, input_dict): + 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 @@ -430,11 +301,11 @@ def _prepare_numerical_time_series(n_shooting, external_forces, input_dict): 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=external_forces["Segment_application"], + 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 = external_forces["with_contact"] if "with_contact" in external_forces.keys() else False + 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 @@ -590,7 +461,7 @@ def _build_parameters( ) @staticmethod - def _build_constraints(model, n_shooting, final_time, control_type, custom_constraint=None, external_forces=None, simultaneous_cycle=1): #, cycling=False): + 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 @@ -604,7 +475,7 @@ def _build_constraints(model, n_shooting, final_time, control_type, custom_const ) if model.muscles_dynamics_model[0].is_approximated: - time_vector = np.linspace(0, final_time*simultaneous_cycle, 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 @@ -647,34 +518,6 @@ def _build_constraints(model, n_shooting, final_time, control_type, custom_const return constraints - @staticmethod - def _build_constraints_for_cycling(constraints, model): - # constraints.add( - # ConstraintFcn.TRACK_MARKERS_VELOCITY, - # node=Node.START, - # marker_index=model.marker_index("wheel_center"), - # axes=[Axis.X, Axis.Y], - # ) - - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - first_marker="wheel_center", - second_marker="global_wheel_center", - node=Node.ALL, - axes=[Axis.X, Axis.Y], - ) - return constraints - - @staticmethod - def _prepare_initial_guess_cycling(biorbd_model_path, n_shooting, x_center, y_center, radius, n_cycles_simultaneous=1,): - biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" #TODO : make it a def entry - q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( - biorbd_model_path, n_shooting * n_cycles_simultaneous, x_center, y_center, radius, ik_method="trf", cycling_number=n_cycles_simultaneous - ) - # u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) - - return q_guess, qdot_guess #, u_guess - @staticmethod def _set_bounds_fes(bio_models): # ---- STATE BOUNDS REPRESENTATION ---- # @@ -771,25 +614,6 @@ def _set_bounds_msk(x_bounds, x_init, bio_models, msk_info): return x_bounds, x_init - @staticmethod - def _set_bounds_msk_for_cycling(x_bounds, x_init, bio_models, initial_guess, n_cycles_simultaneous: int = 1): - q_x_bounds = bio_models.bounds_from_ranges("q") - qdot_x_bounds = bio_models.bounds_from_ranges("qdot") - qdot_x_bounds.max[2] = [0, 0, 0] - - x_bounds.add(key="q", bounds=q_x_bounds, phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) - x_bounds["q"].min[-1, :] = x_bounds["q"].min[-1, :] * n_cycles_simultaneous # Allow the wheel to spin as much as needed - x_bounds["q"].max[-1, :] = x_bounds["q"].max[-1, :] * n_cycles_simultaneous - - x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) - - if initial_guess["q_guess"] is not None: - x_init.add("q", initial_guess["q_guess"], interpolation=InterpolationType.EACH_FRAME) - if initial_guess["qdot_guess"] is not None: - x_init.add("qdot", initial_guess["qdot_guess"], interpolation=InterpolationType.EACH_FRAME) - - return x_bounds, x_init - @staticmethod def _set_u_bounds_fes(bio_models): u_bounds = BoundsList() # Controls bounds @@ -813,28 +637,11 @@ def _set_u_bounds_fes(bio_models): 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 = [-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) - return u_bounds, u_init - - @staticmethod - def _set_u_bounds_msk_for_cycling(u_bounds, u_init, bio_models, with_residual_torque, u_initial_guess): - if with_residual_torque: - nb_tau = bio_models.nb_tau - tau_min, tau_max, tau_init = [-50] * nb_tau, [50] * nb_tau, [0] * nb_tau - tau_min[2] = 0 - tau_max[2] = 0 + 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) - - if u_initial_guess["u_guess"] is not None: - u_init.add("tau", u_initial_guess["u_guess"], interpolation=InterpolationType.EACH_FRAME) - return u_bounds, u_init @staticmethod @@ -895,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[:, np.newaxis].T, - node=Node.ALL, - phase=0, - quadratic=True, - ) - if objective["q_tracking"]: q_fourier_coef = [] for i in range(len(objective["q_tracking"][1])): @@ -1044,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"] - 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") @@ -1095,16 +855,3 @@ def _sanity_check_msk_inputs( if list_to_check[i]: if not isinstance(list_to_check[i], bool): raise TypeError(f"{list_to_check_name[i]} should be a boolean") - - @staticmethod - def _check_if_cycling_objectives_are_feasible(cycling_objective, model): - if "x_center" in cycling_objective and "y_center" in cycling_objective and "radius" in cycling_objective: - with open(model.bio_model.path) as f: - lines = f.readlines() - check_x_center = True in ["$wheel_x_position " + str(cycling_objective["x_center"]) in line for line in lines] - check_y_center = True in ["$wheel_y_position " + str(cycling_objective["y_center"]) in line for line in lines] - check_radius = True in ["$wheel_radius " + str(cycling_objective["radius"]) in line for line in lines] - if not all([check_x_center, check_y_center, check_radius]): - raise ValueError("x_center, y_center and radius should be declared and have the same value in the model") - else: - raise ValueError("cycling_objective should contain x_center, y_center and radius keys") From 64c5cd6cf16a82fd7bed471d7917e39b2df9b0e2 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 31 Jan 2025 14:54:56 -0500 Subject: [PATCH 24/32] removed the for_cycling argument --- cocofest/models/dynamical_model.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index 312480b..0b6593c 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -35,7 +35,6 @@ def __init__( activate_residual_torque: bool = False, parameters: ParameterList = None, external_force_set: ExternalForceSetTimeSeries = None, - for_cycling: bool = False, ): """ The custom model that will be used in the optimal control program for the FES-MSK models @@ -80,7 +79,6 @@ def __init__( self.activate_residual_torque = activate_residual_torque self.parameters_list = parameters self.external_forces_set = external_force_set - self.for_cycling = for_cycling # ---- Absolutely needed methods ---- # def serialize(self) -> tuple[Callable, dict]: From 00546eb5c15811b21769ccd3092b01405fefead7 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Fri, 31 Jan 2025 14:56:19 -0500 Subject: [PATCH 25/32] Custom rk4 integrator to enable rk4 calculation w/ out calcium error --- cocofest/integration/integrator.py | 45 ++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 cocofest/integration/integrator.py 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) + From cf79af2641c9a9e6c3eace7ad0ae4cac5ca9747e Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 4 Feb 2025 11:57:16 -0500 Subject: [PATCH 26/32] nmpc example --- ...c_cycling_with_different_driven_methods.py | 411 ++++++++++++++++++ 1 file changed, 411 insertions(+) create mode 100644 cocofest/examples/dynamics/cycling/nmpc_cycling_with_different_driven_methods.py diff --git a/cocofest/examples/dynamics/cycling/nmpc_cycling_with_different_driven_methods.py b/cocofest/examples/dynamics/cycling/nmpc_cycling_with_different_driven_methods.py new file mode 100644 index 0000000..9ec447e --- /dev/null +++ b/cocofest/examples/dynamics/cycling/nmpc_cycling_with_different_driven_methods.py @@ -0,0 +1,411 @@ +""" +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, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + PhaseDynamics, + Solver, + MultiCyclicNonlinearModelPredictiveControl, + SolutionMerge, + Solution, + MultiCyclicCycleSolutions, +) + +from cocofest import ( + inverse_kinematics_cycling, + FesMskModel, + DingModelPulseWidthFrequencyWithFatigue, + OcpFesMsk, + OcpFes, + CustomObjective, + DingModelPulseWidthFrequency, +) + + +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 + + # self.update_stim(sol) + # self.nlp[0].x_bounds["q"].min[-1, :] = self.nlp[0].x_init["q"].init[0, :] + # 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, :] = [5] + # self.nlp[0].x_bounds["q"].max[-1, :] = self.nlp[0].model.bounds_from_ranges("q").max[-1] * n_cycles_simultaneous + 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): + # 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], 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=3), + n_threads=20, + use_sx=True, + ) + + +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(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100000, quadratic=True) + 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_test_one_marker.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) + # if dynamics_type_str == "fes_driven": + # u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), 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[2] = [-2, -2, -2] + # qdot_x_bounds.min[2] = [-10, -10, -10] + 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] + 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(): + # --- Prepare the ocp --- # + dynamics_type = "fes_driven" + model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod" + pulse_width = None + + cycle_duration = 1 + cycle_len = 33 + n_cycles_to_advance = 1 + n_cycles_simultaneous = 3 + n_cycles = 3 + + if dynamics_type == "torque_driven" or dynamics_type == "muscle_driven": + model = BiorbdModel(model_path) + elif dynamics_type == "fes_driven": + model = FesMskModel( + name=None, + biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", + 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, 100)[:-1]), + activate_force_length_relationship=True, + activate_force_velocity_relationship=True, + activate_residual_torque=False, + external_force_set=None, # External forces will be added + ) + pulse_width = {"min": DingModelPulseWidthFrequencyWithFatigue().pd0, "max": 0.0006, "bimapping": False, "same_for_all_muscles": False, + "fixed": False} + cycle_len = int(OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration*n_cycles_simultaneous) / n_cycles_simultaneous) + else: + raise ValueError("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=3, + pedal_config={"x_center": 0.35, "y_center": 0, "radius": 0.1}, + 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 + + nmpc.add_plot_penalty(CostType.ALL) + # Solve the program + 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, + ) + + 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() From 2b073e0f2232ccdb9143343fe9d9f688717b71e3 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 4 Feb 2025 15:14:55 -0500 Subject: [PATCH 27/32] Cleaning cycling examples --- .../dynamics/cycling/cycling_fes_driven.py | 352 ------------------ .../cycling/cycling_inverse_kinematics.py | 91 +++++ .../cycling_with_different_driven_methods.py | 349 ++++++++++++----- ..._with_different_driven_methods_in_nmpc.py} | 31 +- ...cycling_with_external_force_nmpc_cyclic.py | 261 ------------- ...le_fes_cycling_with_external_force_nmpc.py | 275 -------------- .../fes_cycling_with_external_force_nmpc.py | 105 ------ .../cycling/inverse_kinematics_cycling.py | 73 ---- ...dal_aligned_for_inverse_kinematics.bioMod} | 0 9 files changed, 355 insertions(+), 1182 deletions(-) delete mode 100644 cocofest/examples/dynamics/cycling/cycling_fes_driven.py create mode 100644 cocofest/examples/dynamics/cycling/cycling_inverse_kinematics.py rename cocofest/examples/dynamics/cycling/{nmpc_cycling_with_different_driven_methods.py => cycling_with_different_driven_methods_in_nmpc.py} (93%) delete mode 100644 cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py delete mode 100644 cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py delete mode 100644 cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py delete mode 100644 cocofest/examples/dynamics/cycling/inverse_kinematics_cycling.py rename cocofest/examples/msk_models/{simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod => simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod} (100%) 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 daf31f9..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_fes_driven.py +++ /dev/null @@ -1,352 +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() - - -""" -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 numpy as np - -from bioptim import ( - Axis, - BiorbdModel, - ConstraintFcn, - DynamicsList, - ExternalForceSetTimeSeries, - InterpolationType, - Node, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - PhaseDynamics, - Solver, - MultiCyclicNonlinearModelPredictiveControl, - MultiCyclicCycleSolutions, - Solution, - SolutionMerge, - ControlType, - OptimalControlProgram, -) - -from cocofest import OcpFesMsk, FesMskModel, DingModelPulseWidthFrequency, OcpFes, CustomObjective - - -def prepare_ocp( - model: FesMskModel, - pulse_width: dict, - cycle_duration: int | float, - n_cycles_simultaneous: int, - objective: dict, -): - cycle_len = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration) - total_n_shooting = cycle_len * n_cycles_simultaneous - - # --- EXTERNAL FORCES --- # - total_external_forces_frame = total_n_shooting - external_force_set = ExternalForceSetTimeSeries(nb_frames=total_external_forces_frame) - external_force_array = np.array([0, 0, -1]) - reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, total_external_forces_frame)) - external_force_set.add_torque(segment="wheel", values=reshape_values_array) - - # --- DYNAMICS --- # - numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} - dynamics = DynamicsList() - dynamics.add( - model.declare_model_variables, - dynamic_function=model.muscle_dynamic, - expand_dynamics=True, - expand_continuity=False, - phase=0, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - with_contact=True, - ) - - # --- OBJECTIVE FUNCTION --- # - # 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"] - - objective_functions = ObjectiveList() - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1000, quadratic=True) - # objective_functions.add(CustomObjective.minimize_overall_muscle_force_production, custom_type=ObjectiveFcn.Lagrange, weight=10, quadratic=True) - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=0.0001, quadratic=True) - - # --- BOUNDS AND INITIAL GUESS --- # - # Path constraint: x_bounds, x_init - x_bounds, x_init = OcpFesMsk._set_bounds_fes(model) - q_guess, qdot_guess = OcpFesMsk._prepare_initial_guess_cycling(model.biorbd_path, - cycle_len, - x_center, - y_center, - radius, - n_cycles_simultaneous) - - # import matplotlib.pyplot as plt - # plt.plot(q_guess[0]) - # plt.plot(q_guess[1]) - # plt.show() - objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, key="q", index=0, target=q_guess[0][:-1], weight=1000, - quadratic=True) - objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, key="q", index=1, target=q_guess[1][:-1], weight=1000, - quadratic=True) - - - x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) - x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) - - q_x_bounds = model.bio_model.bounds_from_ranges("q") - q_x_bounds.min[0] = [-1] - q_x_bounds.max[0] = [2] - q_x_bounds.min[1] = [1] - q_x_bounds.min[2] = [q_x_bounds.min[2][0] * n_cycles_simultaneous] - q_x_bounds.max[2] = [5] - - # 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]] * (cycle_len * n_cycles_simultaneous + 1)) - # x_max_bound.append([q_x_bounds.max[i][0]] * (cycle_len * n_cycles_simultaneous + 1)) - # - # # # Resize bounds to reduce search space - # x_min_bound[0] = [-1] * (cycle_len * n_cycles_simultaneous + 1) - # x_max_bound[0] = [2] * (cycle_len * n_cycles_simultaneous + 1) - # x_min_bound[1] = [1] * (cycle_len * n_cycles_simultaneous + 1) - # x_max_bound[2] = [5] * (cycle_len * n_cycles_simultaneous + 1) - # x_min_bound[2] = [x_min_bound[2][0] * n_cycles_simultaneous] * (cycle_len * n_cycles_simultaneous + 1) # Allow the wheel to spin as much as needed - - # cardinal_node_list = [i * int(cycle_len / 4) for i in range(4 * n_cycles_simultaneous + 1)] - # for i in cardinal_node_list: - # x_min_bound[0][i] = x_init["q"].init[0][i] #- x_init["q"].init[0][i] * 0.01 - # x_max_bound[0][i] = x_init["q"].init[0][i] #+ #x_init["q"].init[0][i] * 0.01 - # x_min_bound[1][i] = x_init["q"].init[1][i] #- x_init["q"].init[1][i] * 0.01 - # x_max_bound[1][i] = x_init["q"].init[1][i] #+ x_init["q"].init[1][i] * 0.01 - - # x_bounds.add(key="q", min_bound=x_min_bound, max_bound=x_max_bound, phase=0, - # interpolation=InterpolationType.EACH_FRAME) - - x_bounds.add(key="q", bounds=q_x_bounds, phase=0) - - # Modifying pedal speed bounds - qdot_x_bounds = model.bio_model.bounds_from_ranges("qdot") - qdot_x_bounds.max[2] = [0, 0, 0] - x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) - - # Define control path constraint: u_bounds, u_init - u_bounds, u_init = OcpFesMsk._set_u_bounds_fes(model) - u_bounds, u_init = OcpFesMsk._set_u_bounds_msk(u_bounds, u_init, model, with_residual_torque=True) - u_bounds.add(key="tau", min_bound=np.array([-500, -500, -0]), max_bound=np.array([500, 500, 0]), phase=0, - interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) - - # --- CONSTRAINTS --- # - constraints = OcpFesMsk._build_constraints( - model, - cycle_len, - cycle_duration, - ControlType.CONSTANT, - custom_constraint=None, - external_forces=True, - simultaneous_cycle=n_cycles_simultaneous, - ) - - constraints.add( - ConstraintFcn.TRACK_MARKERS_VELOCITY, - node=Node.ALL, - marker_index=model.marker_index("wheel_center"), - axes=[Axis.X, Axis.Y], - ) - - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - first_marker="wheel_center", - second_marker="global_wheel_center", - node=Node.START, - axes=[Axis.X, Axis.Y], - ) - - # cardinal_node_list = [i * int(cycle_len / 2) for i in range(2 * n_cycles_simultaneous + 1)] - # for i in cardinal_node_list: - # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=0, node=i, weight=10000000, target=q_guess[0][i], quadratic=True) - # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=1, node=i, weight=10000000, target=q_guess[1][i], quadratic=True) - # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=i, target=q_guess[0][i]) - # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=1, node=i, target=q_guess[1][i]) - # constraints.add(ConstraintFcn.TRACK_STATE, key="qdot", index=0, node=i, target=qdot_guess[0][i]) - # constraints.add(ConstraintFcn.TRACK_STATE, key="qdot", index=1, node=i, target=qdot_guess[1][i]) - - # --- PARAMETERS --- # - (parameters, - parameters_bounds, - parameters_init, - parameter_objectives, - ) = OcpFesMsk._build_parameters( - model=model, - pulse_width=pulse_width, - pulse_intensity=None, - use_sx=True, - ) - - # rebuilding model for the OCP - 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, - for_cycling=True, - ) - - return OptimalControlProgram( - [model], - dynamics, - n_shooting=total_n_shooting, - phase_time=3, - 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=3), - control_type=ControlType.CONSTANT, - n_threads=8, - use_sx=True, - ) - - -def main(): - cycle_duration = 1 - n_cycles_simultaneous = 3 - - model = FesMskModel( - name=None, - biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", - muscles_model=[ - DingModelPulseWidthFrequency(muscle_name="DeltoideusClavicle_A", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="DeltoideusScapula_P", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="TRIlong", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="BIC_long", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="BIC_brevis", is_approximated=False), - ], - 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, # External forces will be added - ) - - minimum_pulse_width = DingModelPulseWidthFrequency().pd0 - - ocp = prepare_ocp( - model=model, - pulse_width={ - "min": minimum_pulse_width, - "max": 0.0006, - "bimapping": False, - "same_for_all_muscles": False, - "fixed": False, - }, - cycle_duration=cycle_duration, - n_cycles_simultaneous=n_cycles_simultaneous, - objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}, - "minimize_residual_torque": True}, - ) - - # Solve the program - sol = ocp.solve( - solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), - ) - - sol.graphs(show_bounds=True) - sol.animate(n_frames=200, show_tracked_markers=True) - print(sol.constraints) - print(sol.parameters) - print(sol.detailed_cost) - - -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_with_different_driven_methods.py b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py index 3b8075e..1dc29de 100644 --- a/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py +++ b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py @@ -1,10 +1,9 @@ """ -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. +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 scipy.interpolate import interp1d from bioptim import ( Axis, @@ -23,22 +22,17 @@ ObjectiveList, OdeSolver, OptimalControlProgram, + ParameterList, PhaseDynamics, Solver, - PhaseTransitionList, - PhaseTransitionFcn, - ControlType, ) from cocofest import ( - get_circle_coord, - inverse_kinematics_cycling, - inverse_dynamics_cycling, - FesMskModel, CustomObjective, - DingModelPulseWidthFrequency, + DingModelPulseWidthFrequencyWithFatigue, + FesMskModel, + inverse_kinematics_cycling, OcpFesMsk, - OcpFes, ) @@ -52,33 +46,55 @@ def prepare_ocp( dynamics_type: str = "torque_driven", use_sx: bool = True, ) -> OptimalControlProgram: - - # Dynamics + """ + 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. + + 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) - dynamics = set_dynamics(model=model, numerical_time_series=numerical_time_series, dynamics_type_str=dynamics_type) - - # Define objective functions + # 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) - - # Initial q guess + # Set initial guess for state variables x_init = set_x_init(n_shooting, pedal_config, turn_number) - - # Path constraint - x_bounds = set_bounds(model=model, - x_init=x_init, - n_shooting=n_shooting, - turn_number=turn_number, - interpolation_type=InterpolationType.EACH_FRAME, - cardinal=4) - # x_bounds = set_bounds(bio_model=bio_model, x_init=x_init, n_shooting=n_shooting, interpolation_type=InterpolationType.CONSTANT) - - # Control path constraint - u_bounds, u_init = set_u_bounds_and_init(model, dynamics_type_str=dynamics_type) - - # Constraints + # 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=4 + ) + # 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) - # Parameters + # Configure FES parameters if using an FES model and pulse_width is provided parameters = None parameters_bounds = None parameters_init = None @@ -95,8 +111,8 @@ def prepare_ocp( use_sx=use_sx, ) - # Update model - model = updating_model(model=model, external_force_set=external_force_set, parameters=parameters) + # Update the model with external forces and parameters + model = update_model(model, external_force_set, parameters) return OptimalControlProgram( [model], @@ -108,7 +124,7 @@ def prepare_ocp( x_init=x_init, u_init=u_init, objective_functions=objective_functions, - ode_solver=OdeSolver.RK1(n_integration_steps=20), + ode_solver=OdeSolver.RK1(n_integration_steps=10), n_threads=20, constraints=constraints, parameters=parameters, @@ -119,7 +135,21 @@ def prepare_ocp( ) -def set_external_forces(n_shooting, torque): +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)) @@ -128,7 +158,23 @@ def set_external_forces(n_shooting, torque): return numerical_time_series, external_force_set -def updating_model(model, external_force_set, parameters=None): +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, @@ -141,7 +187,6 @@ def updating_model(model, external_force_set, parameters=None): activate_residual_torque=model.activate_residual_torque, parameters=parameters, external_force_set=external_force_set, - for_cycling=True, ) else: model = BiorbdModel(model.path, external_force_set=external_force_set) @@ -149,7 +194,23 @@ def updating_model(model, external_force_set, parameters=None): return model -def set_dynamics(model, numerical_time_series, dynamics_type_str="torque_driven"): +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" @@ -171,32 +232,60 @@ def set_dynamics(model, numerical_time_series, dynamics_type_str="torque_driven" return dynamics -def set_objective_functions(model, dynamics_type): +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(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100000, quadratic=True) - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_ACCELERATION, marker_index=model.marker_index("hand"), weight=100, quadratic=True) - # objective_functions.add(CustomObjective.minimize_overall_muscle_force_production, custom_type=ObjectiveFcn.Lagrange, weight=1, quadratic=True) + 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) - # if isinstance(model, BiorbdModel): - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_ACCELERATION, - # marker_index=model.marker_index("hand"), weight=100, quadratic=True) - # else: # TO DO: check - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_ACCELERATION, - # marker_index=model.marker_index("hand"), weight=100, quadratic=True) + return objective_functions - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=1, 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. -def set_x_init(n_shooting, pedal_config, turn_number): + Returns + ------- + An InitialGuessList for the state variables. + """ x_init = InitialGuessList() - - biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" - # biorbd_model_path = "../../msk_models/arm26_cycling_pedal_aligned_contact_one_marker.bioMod" + # 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, @@ -207,14 +296,31 @@ def set_x_init(n_shooting, pedal_config, turn_number): 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, dynamics_type_str): +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": @@ -228,29 +334,54 @@ def set_u_bounds_and_init(bio_model, dynamics_type_str): u_init.add(key="muscles", initial_guess=np.array([muscle_init] * bio_model.nb_muscles), phase=0) - if dynamics_type_str == "fes_driven": - u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0) - - return u_bounds, u_init - -def set_bounds(model, x_init, n_shooting, turn_number, interpolation_type=InterpolationType.CONSTANT, cardinal=4): + # 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) + slack = 10*(np.pi/180) # 10 degrees in radians for i in cardinal_node_list: x_min_bound[2][i] = x_init["q"].init[2][i] - slack x_max_bound[2][i] = x_init["q"].init[2][i] + slack @@ -261,19 +392,34 @@ def set_bounds(model, x_init, n_shooting, turn_number, interpolation_type=Interp else: x_bounds.add(key="q", bounds=q_x_bounds, phase=0) - # Modifying pedal speed bounds + # Modify bounds for velocities (e.g., setting maximum pedal speed to 0 to prevent the pedal to go backward) qdot_x_bounds.max[2] = [0, 0, 0] - # qdot_x_bounds.min[2] = [-60, -60, -60] x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) return x_bounds -def set_constraints(bio_model, n_shooting, turn_number): +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=bio_model.marker_index("wheel_center"), + marker_index=model.marker_index("wheel_center"), axes=[Axis.X, Axis.Y], ) @@ -288,62 +434,67 @@ def set_constraints(bio_model, n_shooting, turn_number): axes=[Axis.X, Axis.Y], ) - # cardinal_node_list = [i * int(300 / ((300 / 100) * 2)) for i in - # range(int((300 / 100) * 2 + 1))] - # for i in cardinal_node_list: - # constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=i, target=x_init["q"].init[2][i]) - return constraints def main(): - # --- Prepare the ocp --- # - dynamics_type = "fes_driven" + """ + 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 = 100 final_time = 1 - if dynamics_type == "torque_driven" or dynamics_type == "muscle_driven": + turn_number = 1 + 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, 1, 34)[:-1]) model = FesMskModel( name=None, - biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", - muscles_model=[ - DingModelPulseWidthFrequency(muscle_name="DeltoideusClavicle_A", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="DeltoideusScapula_P", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="TRIlong", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="BIC_long", is_approximated=False), - DingModelPulseWidthFrequency(muscle_name="BIC_brevis", is_approximated=False), - ], - # 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], - # stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], - stim_time=list(np.linspace(0, 1, 34)[:-1]), + 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=True, - external_force_set=None, # External forces will be added + activate_residual_torque=False, + external_force_set=None, # External forces will be added later ) - pulse_width = {"min": DingModelPulseWidthFrequency().pd0, "max": 0.0006, "bimapping": False, "same_for_all_muscles": False, + pulse_width = {"min": DingModelPulseWidthFrequencyWithFatigue().pd0, "max": 0.0006, "bimapping": False, "same_for_all_muscles": False, "fixed": False} - # n_shooting = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, final_time) - n_shooting = 33 + # Adjust n_shooting based on the stimulation time + n_shooting = len(stim_time) + else: - raise ValueError("Dynamics type not recognized") + raise ValueError(f"Dynamics type '{dynamics_type}' not recognized") ocp = prepare_ocp( model=model, n_shooting=n_shooting, final_time=final_time, - turn_number=1, - pedal_config={"x_center": 0.35, "y_center": 0, "radius": 0.1}, + turn_number=turn_number, + pedal_config=pedal_config, pulse_width=pulse_width, dynamics_type=dynamics_type, ) + # Add the penalty cost function plot ocp.add_plot_penalty(CostType.ALL) - sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) #, show_options=dict(show_bounds=True)))#, show_options=dict(show_bounds=True))) + # Solve the optimal control problem + sol = ocp.solve(Solver.IPOPT(show_online_optim=False, _max_iter=10000)) + # Display graphs and animate the solution sol.graphs(show_bounds=True) sol.animate(viewer="pyorerun") diff --git a/cocofest/examples/dynamics/cycling/nmpc_cycling_with_different_driven_methods.py b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods_in_nmpc.py similarity index 93% rename from cocofest/examples/dynamics/cycling/nmpc_cycling_with_different_driven_methods.py rename to cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods_in_nmpc.py index 9ec447e..df04fb8 100644 --- a/cocofest/examples/dynamics/cycling/nmpc_cycling_with_different_driven_methods.py +++ b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods_in_nmpc.py @@ -49,11 +49,8 @@ def advance_window_bounds_states(self, sol, n_cycles_simultaneous=None, **extra) self.nlp[0].x_bounds["q"].min[-1, 0] = min_pedal_bound self.nlp[0].x_bounds["q"].max[-1, 0] = max_pedal_bound - # self.update_stim(sol) - # self.nlp[0].x_bounds["q"].min[-1, :] = self.nlp[0].x_init["q"].init[0, :] - # 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, :] = [5] - # self.nlp[0].x_bounds["q"].max[-1, :] = self.nlp[0].model.bounds_from_ranges("q").max[-1] * n_cycles_simultaneous + if sol.parameters != {}: + self.update_stim(sol) return True def advance_window_initial_guess_states(self, sol, n_cycles_simultaneous=None): @@ -64,11 +61,11 @@ def advance_window_initial_guess_states(self, sol, n_cycles_simultaneous=None): return True 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) + 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 = {} 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 + 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 @@ -224,7 +221,7 @@ def set_objective_functions(model, dynamics_type): def set_x_init(n_shooting, pedal_config, turn_number): x_init = InitialGuessList() - biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" + 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, @@ -299,12 +296,12 @@ def set_bounds(model, x_init, n_shooting, turn_number, interpolation_type=Interp x_bounds.add(key="q", bounds=q_x_bounds, phase=0) # Modifying pedal speed bounds - # qdot_x_bounds.max[2] = [-2, -2, -2] - # qdot_x_bounds.min[2] = [-10, -10, -10] 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] = [-10, -10, -10] x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0) return x_bounds @@ -322,7 +319,6 @@ def set_constraints(bio_model, n_shooting, turn_number): node=i, axes=[Axis.X, Axis.Y], ) - constraints.add( ConstraintFcn.TRACK_MARKERS_VELOCITY, node=i, @@ -342,7 +338,7 @@ def main(): cycle_duration = 1 cycle_len = 33 n_cycles_to_advance = 1 - n_cycles_simultaneous = 3 + n_cycles_simultaneous = 2 n_cycles = 3 if dynamics_type == "torque_driven" or dynamics_type == "muscle_driven": @@ -358,7 +354,7 @@ def main(): 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, 100)[:-1]), + stim_time=list(np.linspace(0, cycle_duration*n_cycles_simultaneous, 67)[:-1]), activate_force_length_relationship=True, activate_force_velocity_relationship=True, activate_residual_torque=False, @@ -366,7 +362,8 @@ def main(): ) pulse_width = {"min": DingModelPulseWidthFrequencyWithFatigue().pd0, "max": 0.0006, "bimapping": False, "same_for_all_muscles": False, "fixed": False} - cycle_len = int(OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration*n_cycles_simultaneous) / n_cycles_simultaneous) + # cycle_len = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration*n_cycles_simultaneous) + cycle_len = 66 else: raise ValueError("Dynamics type not recognized") @@ -377,7 +374,7 @@ def main(): n_cycles_to_advance=n_cycles_to_advance, n_cycles_simultaneous=n_cycles_simultaneous, total_n_cycles=n_cycles, - turn_number=3, + turn_number=1, pedal_config={"x_center": 0.35, "y_center": 0, "radius": 0.1}, pulse_width=pulse_width, dynamics_type=dynamics_type, diff --git a/cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py b/cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py deleted file mode 100644 index b9be157..0000000 --- a/cocofest/examples/dynamics/cycling/cycling_with_external_force_nmpc_cyclic.py +++ /dev/null @@ -1,261 +0,0 @@ -""" -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 platform -import numpy as np - -from bioptim import ( - Axis, - BiorbdModel, - BoundsList, - ConstraintList, - ConstraintFcn, - CostType, - DynamicsFcn, - DynamicsList, - ExternalForceSetTimeSeries, - InitialGuessList, - InterpolationType, - Node, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - OptimalControlProgram, - PhaseDynamics, - Solver, - PhaseTransitionList, - PhaseTransitionFcn, - MultiCyclicNonlinearModelPredictiveControl, - Dynamics, - Objective, - Solution, - SolutionMerge, - MultiCyclicCycleSolutions, - ControlType, -) - -from cocofest import ( - get_circle_coord, - inverse_kinematics_cycling, - inverse_dynamics_cycling, -) - - -class MyCyclicNMPC(MultiCyclicNonlinearModelPredictiveControl): - def advance_window_bounds_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_bounds_states(sol) # Allow the wheel to spin as much as needed - 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 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[-1, :] = q[-1, :] # Keep the previously found value for the wheel - return True - - -def prepare_nmpc( - biorbd_model_path: str, - cycle_len: int, - cycle_duration: int | float, - n_cycles_to_advance: int, - n_cycles_simultaneous: int, - total_n_cycles: int, - objective: dict, - initial_guess_warm_start: bool = False, - dynamics_torque_driven: bool = True, - with_residual_torque: bool = False, - -): - if with_residual_torque and dynamics_torque_driven: - raise ValueError("Residual torque is only available for muscle driven dynamics") - - total_n_shooting = cycle_len * n_cycles_simultaneous - - # External forces - total_external_forces_frame = total_n_cycles * cycle_len if total_n_cycles >= n_cycles_simultaneous else total_n_shooting - external_force_set = ExternalForceSetTimeSeries(nb_frames=total_external_forces_frame) - external_force_array = np.array([0, 0, -1]) - reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, total_external_forces_frame)) - external_force_set.add_torque(segment="wheel", values=reshape_values_array) - - # Dynamics - numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} - bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) - bio_model.current_n_cycles = 1 - - # 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"] - - from scipy.interpolate import interp1d - f = interp1d(np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), - np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), kind="linear") - x_new = f(np.linspace(0, -360 * n_cycles_simultaneous, total_n_shooting + 1)) - x_new_rad = np.deg2rad(x_new) - - circle_coord_list = np.array( - [ - get_circle_coord(theta, x_center, y_center, radius)[:-1] - for theta in x_new_rad - ] - ).T - - objective_functions = ObjectiveList() - for i in [5, 10, 15, 20]: - objective_functions.add( - ObjectiveFcn.Mayer.TRACK_MARKERS, - weight=100000, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list[:, i], - node=i, - phase=0, - quadratic=True, - ) - - control_key = "tau" if dynamics_torque_driven else "muscles" - weight = 100 if dynamics_torque_driven else 1 - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key=control_key, weight=weight, quadratic=True) - if with_residual_torque: - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, quadratic=True) - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=0.0001, quadratic=True) - - # Dynamics - chosen_dynamics = DynamicsFcn.TORQUE_DRIVEN if dynamics_torque_driven else DynamicsFcn.MUSCLE_DRIVEN - dynamics = DynamicsList() - dynamics.add( - chosen_dynamics, - expand_dynamics=True, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - with_contact=True, - # with_residual_torque=with_residual_torque, - ) - - # 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["q"].min[-1, :] = x_bounds["q"].min[-1, :] * n_cycles_simultaneous # Allow the wheel to spin as much as needed - x_bounds["q"].max[-1, :] = x_bounds["q"].max[-1, :] * n_cycles_simultaneous - - # Modifying pedal speed bounds - qdot_x_bounds.max[2] = [0, 0, 0] - x_bounds.add(key="qdot", bounds=qdot_x_bounds, phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,) - - # Define control path constraint - u_bounds = BoundsList() - if dynamics_torque_driven: - u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,) - else: - if with_residual_torque: - u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,) - muscle_min, muscle_max, muscle_init = 0.0, 1.0, 0.5 - u_bounds["muscles"] = [muscle_min] * bio_model.nb_muscles, [muscle_max] * bio_model.nb_muscles - u_init = InitialGuessList() - u_init["muscles"] = [muscle_init] * 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 and dynamics - if initial_guess_warm_start: - biorbd_model_path = "../../msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod" - q_guess, qdot_guess, qddotguess = inverse_kinematics_cycling( - biorbd_model_path, total_n_shooting, x_center, y_center, radius, ik_method="trf", cycling_number=n_cycles_simultaneous - ) - x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME) - x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME) - # if not dynamics_torque_driven and with_residual_torque: - # u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess) - # u_init.add("tau", u_guess, interpolation=InterpolationType.EACH_FRAME) - - # Constraints - constraints = ConstraintList() - - constraints.add( - ConstraintFcn.TRACK_MARKERS_VELOCITY, - node=Node.START, - marker_index=bio_model.marker_index("wheel_center"), - axes=[Axis.X, Axis.Y], - ) - - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - first_marker="wheel_center", - second_marker="global_wheel_center", - node=Node.START, - axes=[Axis.X, Axis.Y], - ) - - return MyCyclicNMPC( - [bio_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, - ode_solver=OdeSolver.RK4(), - n_threads=8, - ) - - -def main(): - cycle_duration = 1 - cycle_len = 20 - n_cycles_to_advance = 1 - n_cycles_simultaneous = 2 - n_cycles = 2 - - nmpc = prepare_nmpc( - biorbd_model_path="../../msk_models/simplified_UL_Seth_pedal_aligned_test.bioMod", - cycle_len=cycle_len, - cycle_duration=cycle_duration, - n_cycles_to_advance=n_cycles_to_advance, - n_cycles_simultaneous=n_cycles_simultaneous, - total_n_cycles=n_cycles, - objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}}, - initial_guess_warm_start=True, - dynamics_torque_driven=True, - with_residual_torque=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 - - # Solve the program - sol = nmpc.solve( - update_functions, - solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), - n_cycles_simultaneous=n_cycles_simultaneous, - cyclic_options={"states": {}}, - cycle_solutions=MultiCyclicCycleSolutions.ALL_CYCLES, - get_all_iterations=True, - ) - - # sol.print_cost() - # sol.graphs(show_bounds=True) - sol[1][0].graphs(show_bounds=True) - sol[1][1].graphs(show_bounds=True) - - sol[0].graphs(show_bounds=True) - sol[0].animate(n_frames=100) - # sol.animate(n_frames=200, show_tracked_markers=True) - - -if __name__ == "__main__": - main() diff --git a/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py b/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py deleted file mode 100644 index 72ece8f..0000000 --- a/cocofest/examples/dynamics/cycling/example_fes_cycling_with_external_force_nmpc.py +++ /dev/null @@ -1,275 +0,0 @@ -""" -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 numpy as np - -from bioptim import ( - Axis, - ConstraintFcn, - DynamicsList, - ExternalForceSetTimeSeries, - InterpolationType, - Node, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - PhaseDynamics, - Solver, - MultiCyclicNonlinearModelPredictiveControl, - Solution, - SolutionMerge, - ControlType, -) - -from cocofest import get_circle_coord, OcpFesMsk, FesMskModel, DingModelPulseWidthFrequencyWithFatigue, OcpFes - - -class MyCyclicNMPC(MultiCyclicNonlinearModelPredictiveControl): - def advance_window_bounds_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_bounds_states(sol) # Allow the wheel to spin as much as needed - 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 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[-1, :] = q[-1, :] # Keep the previously found value for the wheel - return True - - -def prepare_nmpc( - model: FesMskModel, - stim_time: list, - pulse_width: dict, - cycle_duration: int | float, - n_cycles_to_advance: int, - n_cycles_simultaneous: int, - total_n_cycles: int, - objective: dict, -): - # cycle_len = OcpFes.prepare_n_shooting(stim_time, cycle_duration) - cycle_len = 20 - total_n_shooting = cycle_len * n_cycles_simultaneous - - # --- EXTERNAL FORCES --- # - total_external_forces_frame = total_n_cycles * cycle_len if total_n_cycles >= n_cycles_simultaneous else total_n_shooting - external_force_set = ExternalForceSetTimeSeries(nb_frames=total_external_forces_frame) - external_force_array = np.array([0, 0, -1]) - reshape_values_array = np.tile(external_force_array[:, np.newaxis], (1, total_external_forces_frame)) - external_force_set.add_torque(segment="wheel", values=reshape_values_array) - - # --- OBJECTIVE FUNCTION --- # - # 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"] - - from scipy.interpolate import interp1d - f = interp1d(np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), - np.linspace(0, -360 * n_cycles_simultaneous, 360 * n_cycles_simultaneous + 1), kind="linear") - x_new = f(np.linspace(0, -360 * n_cycles_simultaneous, total_n_shooting + 1)) - x_new_rad = np.deg2rad(x_new) - - circle_coord_list = np.array( - [ - get_circle_coord(theta, x_center, y_center, radius)[:-1] - for theta in x_new_rad - ] - ).T - - objective_functions = ObjectiveList() - objective_functions.add( - ObjectiveFcn.Mayer.TRACK_MARKERS, - weight=100000, - axes=[Axis.X, Axis.Y], - marker_index=0, - target=circle_coord_list, - node=Node.ALL, - phase=0, - quadratic=True, - ) - - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, quadratic=True) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTACT_FORCES, weight=0.0001, quadratic=True) - - # --- DYNAMICS --- # - numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} - dynamics = DynamicsList() - dynamics.add( - model.declare_model_variables, - dynamic_function=model.muscle_dynamic, - expand_dynamics=True, - expand_continuity=False, - phase=0, - phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, - numerical_data_timeseries=numerical_time_series, - with_contact=True, - ) - - # --- BOUNDS AND INITIAL GUESS --- # - # Path constraint: x_bounds, x_init - x_bounds, x_init = OcpFesMsk._set_bounds_fes(model) - x_bounds, x_init = OcpFesMsk._set_bounds_msk(x_bounds, x_init, model, msk_info={"bound_type": None}) - - q_guess, qdot_guess = OcpFesMsk._prepare_initial_guess_cycling(model.biorbd_path, - cycle_len, - x_center, - y_center, - radius, - n_cycles_simultaneous) - - x_initial_guess = {"q_guess": q_guess, "qdot_guess": qdot_guess} - x_bounds, x_init = OcpFesMsk._set_bounds_msk_for_cycling(x_bounds, x_init, model, x_initial_guess, - n_cycles_simultaneous) - - # Define control path constraint: u_bounds, u_init - u_bounds, u_init = OcpFesMsk._set_u_bounds_fes(model) - u_bounds, u_init = OcpFesMsk._set_u_bounds_msk(u_bounds, u_init, model, with_residual_torque=True) - u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0, - interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) - - # --- CONSTRAINTS --- # - constraints = OcpFesMsk._build_constraints( - model, - cycle_len, - cycle_duration, - stim_time, - ControlType.CONSTANT, - custom_constraint=None, - external_forces=True, - simultaneous_cycle=n_cycles_simultaneous, - ) - - # constraints.add( - # ConstraintFcn.TRACK_MARKERS_VELOCITY, - # node=Node.START, - # marker_index=model.marker_index("wheel_center"), - # axes=[Axis.X, Axis.Y], - # ) - - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - first_marker="wheel_center", - second_marker="global_wheel_center", - node=Node.ALL, - axes=[Axis.X, Axis.Y], - ) - - # --- PARAMETERS --- # - (parameters, - parameters_bounds, - parameters_init, - parameter_objectives, - ) = OcpFesMsk._build_parameters( - model=model, - stim_time=stim_time, - pulse_event=None, - pulse_width=pulse_width, - pulse_intensity=None, - use_sx=True, - ) - - # rebuilding model for the OCP - model = FesMskModel( - name=model.name, - biorbd_path=model.biorbd_path, - muscles_model=model.muscles_dynamics_model, - 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, - for_cycling=True, - ) - - 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.RK4(), - control_type=ControlType.CONSTANT, - n_threads=8, - use_sx=True, - ) - - -def main(): - cycle_duration = 1 - n_cycles_to_advance = 1 - n_cycles_simultaneous = 2 - n_cycles = 2 - - model = FesMskModel( - name=None, - biorbd_path="../../msk_models/simplified_UL_Seth_pedal_aligned_one_muscle.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 - - nmpc = 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], - # stim_time=[0], - pulse_width={ - "min": minimum_pulse_width, - "max": 0.0006, - "bimapping": False, - "same_for_all_muscles": False, - "fixed": False, - }, - cycle_duration=cycle_duration, - n_cycles_to_advance=n_cycles_to_advance, - n_cycles_simultaneous=n_cycles_simultaneous, - total_n_cycles=n_cycles, - objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}, - "minimize_residual_torque": True}, - ) - - def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): - return cycle_idx < n_cycles # True if there are still some cycle to perform - - # Solve the program - sol = nmpc.solve( - update_functions, - solver=Solver.IPOPT(show_online_optim=False, _max_iter=1000, show_options=dict(show_bounds=True)), - n_cycles_simultaneous=n_cycles_simultaneous, - # get_all_iterations=True, - cyclic_options={"states": {}}, - ) - - sol.print_cost() - sol.graphs(show_bounds=True) - sol.animate(n_frames=200, show_tracked_markers=True) - - -if __name__ == "__main__": - main() diff --git a/cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py b/cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py deleted file mode 100644 index fa736b6..0000000 --- a/cocofest/examples/dynamics/cycling/fes_cycling_with_external_force_nmpc.py +++ /dev/null @@ -1,105 +0,0 @@ -""" -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 platform -import numpy as np - -from bioptim import ( - Axis, - BiorbdModel, - BoundsList, - ConstraintList, - ConstraintFcn, - CostType, - DynamicsFcn, - DynamicsList, - ExternalForceSetTimeSeries, - InitialGuessList, - InterpolationType, - Node, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - OptimalControlProgram, - PhaseDynamics, - Solver, - PhaseTransitionList, - PhaseTransitionFcn, - MultiCyclicNonlinearModelPredictiveControl, - Dynamics, - Objective, - Solution, - SolutionMerge, - MultiCyclicCycleSolutions, - ControlType, -) - -from cocofest import ( - get_circle_coord, - inverse_kinematics_cycling, - inverse_dynamics_cycling, -) - -from cocofest import NmpcFes, OcpFes, FesMskModel, DingModelPulseWidthFrequencyWithFatigue, SolutionToPickle, PickleAnimate, NmpcFesMsk - - -def main(): - 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 - n_cycles = 2 - nmpc_fes_msk = NmpcFesMsk - nmpc = nmpc_fes_msk.prepare_nmpc_for_cycling( - model=model, - stim_time=[0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9], - cycle_duration=1, - n_cycles_to_advance=1, - n_cycles_simultaneous=3, - n_total_cycles=n_cycles, - pulse_width={ - "min": minimum_pulse_width, - "max": 0.0006, - "bimapping": False, - }, - objective={"cycling": {"x_center": 0.35, "y_center": 0, "radius": 0.1}, - "minimize_residual_torque": True}, - msk_info={"with_residual_torque": True}, - external_forces={"torque": [0, 0, -1], "Segment_application": "wheel"} - ) - - 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( - update_functions, - solver=Solver.IPOPT(), - cyclic_options={"states": {}}, - ) - - sol.print_cost() - sol.graphs(show_bounds=True) - sol.animate(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/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod b/cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod similarity index 100% rename from cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_test_one_marker.bioMod rename to cocofest/examples/msk_models/simplified_UL_Seth_pedal_aligned_for_inverse_kinematics.bioMod From c75b304286ccf97773bae67dcc4a9b9c3b6844c1 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 4 Feb 2025 15:27:15 -0500 Subject: [PATCH 28/32] cleaning --- ...g_with_different_driven_methods_in_nmpc.py | 78 +++++++++++-------- 1 file changed, 44 insertions(+), 34 deletions(-) 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 index df04fb8..b4e3b54 100644 --- 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 @@ -17,27 +17,26 @@ ExternalForceSetTimeSeries, InitialGuessList, InterpolationType, - Node, + MultiCyclicCycleSolutions, + MultiCyclicNonlinearModelPredictiveControl, ObjectiveFcn, ObjectiveList, OdeSolver, - OptimalControlProgram, PhaseDynamics, - Solver, - MultiCyclicNonlinearModelPredictiveControl, SolutionMerge, Solution, - MultiCyclicCycleSolutions, + Solver, ) from cocofest import ( - inverse_kinematics_cycling, - FesMskModel, - DingModelPulseWidthFrequencyWithFatigue, - OcpFesMsk, - OcpFes, CustomObjective, DingModelPulseWidthFrequency, + DingModelPulseWidthFrequencyWithFatigue, + FesMskModel, + inverse_kinematics_cycling, + OcpFesMsk, + + ) @@ -209,7 +208,6 @@ def set_dynamics(model, numerical_time_series, dynamics_type_str="torque_driven" def set_objective_functions(model, dynamics_type): objective_functions = ObjectiveList() if isinstance(model, FesMskModel): - # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100000, quadratic=True) 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: @@ -253,9 +251,6 @@ def set_u_bounds_and_init(bio_model, dynamics_type_str): u_init.add(key="muscles", initial_guess=np.array([muscle_init] * bio_model.nb_muscles), phase=0) - # if dynamics_type_str == "fes_driven": - # u_bounds.add(key="tau", min_bound=np.array([-50, -50, -0]), max_bound=np.array([50, 50, 0]), phase=0) - return u_bounds, u_init @@ -330,42 +325,55 @@ def set_constraints(bio_model, n_shooting, turn_number): def main(): - # --- Prepare the ocp --- # - dynamics_type = "fes_driven" + """ + 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 + # NMPC parameters cycle_duration = 1 cycle_len = 33 n_cycles_to_advance = 1 n_cycles_simultaneous = 2 n_cycles = 3 - if dynamics_type == "torque_driven" or dynamics_type == "muscle_driven": + # 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="../../msk_models/simplified_UL_Seth_pedal_aligned.bioMod", - 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]), + 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 + 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, + pulse_width = {"min": DingModelPulseWidthFrequencyWithFatigue().pd0, "max": 0.0006, "bimapping": False, + "same_for_all_muscles": False, "fixed": False} - # cycle_len = OcpFes.prepare_n_shooting(model.muscles_dynamics_model[0].stim_time, cycle_duration*n_cycles_simultaneous) - cycle_len = 66 + # Adjust n_shooting based on the stimulation time + cycle_len = len(stim_time) else: - raise ValueError("Dynamics type not recognized") + raise ValueError(f"Dynamics type '{dynamics_type}' not recognized") nmpc = prepare_nmpc( model=model, @@ -374,8 +382,8 @@ def main(): n_cycles_to_advance=n_cycles_to_advance, n_cycles_simultaneous=n_cycles_simultaneous, total_n_cycles=n_cycles, - turn_number=1, - pedal_config={"x_center": 0.35, "y_center": 0, "radius": 0.1}, + turn_number=turn_number, + pedal_config=pedal_config, pulse_width=pulse_width, dynamics_type=dynamics_type, ) @@ -383,8 +391,9 @@ def main(): 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 program + # 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)), @@ -394,6 +403,7 @@ def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_id 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) From f979b92250c9290e8c44f3e42917619690a4ae0a Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Tue, 4 Feb 2025 15:30:43 -0500 Subject: [PATCH 29/32] get pulse width --- cocofest/models/ding2007.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cocofest/models/ding2007.py b/cocofest/models/ding2007.py index 59d9ae0..944e1ed 100644 --- a/cocofest/models/ding2007.py +++ b/cocofest/models/ding2007.py @@ -272,7 +272,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]) From 851a5d37472aad2f54d4791363fbc384f40ba03e Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Wed, 5 Feb 2025 17:57:01 -0500 Subject: [PATCH 30/32] Passive force implementation --- cocofest/models/ding2003.py | 14 ++++++++- cocofest/models/ding2003_with_fatigue.py | 8 +++++ cocofest/models/ding2007.py | 8 +++++ cocofest/models/ding2007_with_fatigue.py | 9 +++++- cocofest/models/dynamical_model.py | 21 ++++++++++---- cocofest/models/fes_model.py | 3 ++ cocofest/models/hill_coefficients.py | 37 ++++++++++++++++++++++++ cocofest/models/hmed2018.py | 8 +++++ cocofest/models/hmed2018_with_fatigue.py | 6 ++++ 9 files changed, 107 insertions(+), 7 deletions(-) diff --git a/cocofest/models/ding2003.py b/cocofest/models/ding2003.py index 2225d78..9705c86 100644 --- a/cocofest/models/ding2003.py +++ b/cocofest/models/ding2003.py @@ -160,6 +160,7 @@ def system_dynamics( 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. @@ -178,6 +179,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 ------- @@ -193,6 +196,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) @@ -288,6 +292,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 @@ -306,6 +311,8 @@ def f_dot_fun( 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 ------- @@ -313,8 +320,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 @@ -329,6 +337,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 @@ -355,6 +364,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 @@ -377,6 +388,7 @@ def dynamics( cn_sum=cn_sum, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, + passive_force_relationship=passive_force_relationship, ), ) diff --git a/cocofest/models/ding2003_with_fatigue.py b/cocofest/models/ding2003_with_fatigue.py index 011d8fd..1357e3f 100644 --- a/cocofest/models/ding2003_with_fatigue.py +++ b/cocofest/models/ding2003_with_fatigue.py @@ -146,6 +146,7 @@ def system_dynamics( 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. @@ -170,6 +171,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 ------- @@ -185,6 +188,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 @@ -248,6 +252,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 @@ -274,6 +279,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 @@ -299,6 +306,7 @@ def dynamics( t=time, 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/ding2007.py b/cocofest/models/ding2007.py index 944e1ed..0b20e2f 100644 --- a/cocofest/models/ding2007.py +++ b/cocofest/models/ding2007.py @@ -126,6 +126,7 @@ def system_dynamics( 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. @@ -148,6 +149,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 ------- @@ -176,6 +179,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) @@ -290,6 +294,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 @@ -316,6 +321,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 @@ -345,6 +352,7 @@ def dynamics( a_scale=a_scale, 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/ding2007_with_fatigue.py b/cocofest/models/ding2007_with_fatigue.py index 6276d63..f0282bf 100644 --- a/cocofest/models/ding2007_with_fatigue.py +++ b/cocofest/models/ding2007_with_fatigue.py @@ -145,6 +145,7 @@ def system_dynamics( 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. @@ -169,11 +170,12 @@ def system_dynamics( 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 ------- @@ -202,6 +204,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) @@ -266,6 +269,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 @@ -292,6 +296,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 @@ -329,6 +335,7 @@ def dynamics( a_scale=a_scale, 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/dynamical_model.py b/cocofest/models/dynamical_model.py index 0b6593c..b7cfd27 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -19,6 +19,7 @@ from .hill_coefficients import ( muscle_force_length_coefficient, muscle_force_velocity_coefficient, + muscle_passive_force_coefficient, ) @@ -129,7 +130,6 @@ def muscle_dynamic( nlp: NonLinearProgram, muscle_models: list[FesModel], state_name_list=None, - external_forces=None, ) -> DynamicsEvaluation: """ The custom dynamics function that provides the derivative of the states: dxdt = f(t, x, u, p, s) @@ -154,8 +154,6 @@ def muscle_dynamic( The list of the muscle models state_name_list: list[str] The states names list - external_forces: MX | SX - The external forces acting on the system Returns ------- The derivative of the states in the tuple[MX | SX] format @@ -183,9 +181,8 @@ def muscle_dynamic( dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) total_torque = muscles_tau + tau if self.activate_residual_torque else muscles_tau external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_data_timeseries) - with_contact = True if nlp.model.bio_model.contact_names != () else False + 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 - # TODO: Add a better way of with_contact=True dxdt = vertcat(dxdt_muscle_list, dq, ddq) return DynamicsEvaluation(dxdt=dxdt, defects=None) @@ -263,6 +260,19 @@ 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, @@ -274,6 +284,7 @@ def muscles_joint_torque( 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) diff --git a/cocofest/models/fes_model.py b/cocofest/models/fes_model.py index 87e96c3..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, ): """ 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 81fdd81..01921b4 100644 --- a/cocofest/models/hmed2018.py +++ b/cocofest/models/hmed2018.py @@ -126,6 +126,7 @@ def system_dynamics( 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. @@ -146,6 +147,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 ------- @@ -163,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) @@ -228,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 @@ -254,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 @@ -280,6 +287,7 @@ def dynamics( 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 edb8d64..206f4fa 100644 --- a/cocofest/models/hmed2018_with_fatigue.py +++ b/cocofest/models/hmed2018_with_fatigue.py @@ -135,6 +135,7 @@ def system_dynamics( 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. @@ -161,6 +162,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 ------- @@ -178,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 @@ -241,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 @@ -296,6 +301,7 @@ def dynamics( cn_sum=cn_sum, force_length_relationship=force_length_relationship, force_velocity_relationship=force_velocity_relationship, + passive_force_relationship=passive_force_relationship, ), defects=None, ) From c428a69320c0f2a96d97fc4e583abe694b8a3d87 Mon Sep 17 00:00:00 2001 From: Anais Date: Wed, 5 Feb 2025 18:25:49 -0500 Subject: [PATCH 31/32] working on nmpc --- .../cycling_with_different_driven_methods.py | 69 +++++++++++++------ ...g_with_different_driven_methods_in_nmpc.py | 14 ++-- 2 files changed, 56 insertions(+), 27 deletions(-) diff --git a/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py index 1dc29de..c11f0ba 100644 --- a/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py +++ b/cocofest/examples/dynamics/cycling/cycling_with_different_driven_methods.py @@ -45,6 +45,7 @@ def prepare_ocp( 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. @@ -67,6 +68,8 @@ def prepare_ocp( 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 ------- @@ -87,7 +90,7 @@ def prepare_ocp( n_shooting=n_shooting, turn_number=turn_number, interpolation_type=InterpolationType.EACH_FRAME, - cardinal=4 + cardinal=1 ) # Define control bounds and initial guess u_init, u_bounds = set_u_bounds_and_init(model, dynamics_type_str=dynamics_type) @@ -124,7 +127,7 @@ def prepare_ocp( x_init=x_init, u_init=u_init, objective_functions=objective_functions, - ode_solver=OdeSolver.RK1(n_integration_steps=10), + ode_solver=OdeSolver.RK1(n_integration_steps=integration_step), n_threads=20, constraints=constraints, parameters=parameters, @@ -380,11 +383,23 @@ def set_state_bounds(model: BiorbdModel | FesMskModel, x_init: InitialGuessList, 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) # 10 degrees in radians - for i in cardinal_node_list: - x_min_bound[2][i] = x_init["q"].init[2][i] - slack - x_max_bound[2][i] = x_init["q"].init[2][i] + slack + 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) @@ -393,7 +408,12 @@ def set_state_bounds(model: BiorbdModel | FesMskModel, x_init: InitialGuessList, 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[2] = [0, 0, 0] + 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 @@ -416,12 +436,12 @@ def set_constraints(model: BiorbdModel | FesMskModel, n_shooting: int, turn_numb 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], - ) + # 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))] @@ -433,6 +453,12 @@ def set_constraints(model: BiorbdModel | FesMskModel, n_shooting: int, turn_numb 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 @@ -445,14 +471,15 @@ def main(): 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 = 100 - final_time = 1 - turn_number = 1 + 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 = [ @@ -462,7 +489,7 @@ def main(): 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, 1, 34)[:-1]) + stim_time = list(np.linspace(0, 3, 100)[:-1]) model = FesMskModel( name=None, biorbd_path=model_path, @@ -477,7 +504,7 @@ def main(): "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") @@ -489,11 +516,13 @@ def main(): 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=10000)) + 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") 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 index b4e3b54..c0b760c 100644 --- 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 @@ -148,9 +148,9 @@ def prepare_nmpc( parameter_bounds=parameters_bounds, parameter_init=parameters_init, parameter_objectives=parameter_objectives, - ode_solver=OdeSolver.RK1(n_integration_steps=3), + ode_solver=OdeSolver.RK1(n_integration_steps=1), n_threads=20, - use_sx=True, + use_sx=False, ) @@ -295,8 +295,8 @@ def set_bounds(model, x_init, n_shooting, turn_number, interpolation_type=Interp 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] = [-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 @@ -329,15 +329,15 @@ 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" + 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 = 33 + cycle_len = 100 n_cycles_to_advance = 1 - n_cycles_simultaneous = 2 + n_cycles_simultaneous = 3 n_cycles = 3 # Bike parameters From 8535d1e4e5971de3116b27a80c2fd59d5ecf2a96 Mon Sep 17 00:00:00 2001 From: Kevin CO Date: Wed, 5 Feb 2025 18:27:10 -0500 Subject: [PATCH 32/32] Passive force --- cocofest/models/dynamical_model.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/cocofest/models/dynamical_model.py b/cocofest/models/dynamical_model.py index b7cfd27..f5cb417 100644 --- a/cocofest/models/dynamical_model.py +++ b/cocofest/models/dynamical_model.py @@ -33,6 +33,7 @@ def __init__( 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, @@ -52,6 +53,8 @@ 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 @@ -77,6 +80,7 @@ def __init__( 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 @@ -93,6 +97,7 @@ def serialize(self) -> tuple[Callable, dict]: "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,