From eafa832a705ea70b89420133978231aaca927145 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 11:13:43 -0400 Subject: [PATCH 01/18] breaking down optimal control program --- .../optimization/optimal_control_program.py | 192 ++++++++---------- .../stochastic_optimal_control_program.py | 37 ++-- 2 files changed, 100 insertions(+), 129 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 75bcabbb7..48adbf4d7 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -293,6 +293,16 @@ def __init__( integrated_value_functions, ) + self._check_and_set_threads(n_threads) + self._check_and_set_shooting_points(n_shooting) + self._check_and_set_phase_time(phase_time) + + x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling) + u_bounds, u_init, u_scaling = self._check_and_prepare_decision_variables("u", u_bounds, u_init, u_scaling) + s_bounds, s_init, s_scaling = self._check_and_prepare_decision_variables("s", s_bounds, s_init, s_scaling) + + xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) + ( constraints, objective_functions, @@ -301,29 +311,10 @@ def __init__( multinode_constraints, multinode_objectives, phase_transitions, - x_bounds, - u_bounds, parameter_bounds, - s_bounds, - x_init, - u_init, parameter_init, - s_init, ) = self._check_arguments_and_build_nlp( dynamics, - n_threads, - n_shooting, - phase_time, - x_bounds, - u_bounds, - s_bounds, - x_init, - u_init, - s_init, - x_scaling, - xdot_scaling, - u_scaling, - s_scaling, objective_functions, constraints, parameters, @@ -345,6 +336,16 @@ def __init__( variable_mappings, integrated_value_functions, ) + + # Do not copy singleton since x_scaling was already dealt with before + NLP.add(self, "x_scaling", x_scaling, True) + NLP.add(self, "xdot_scaling", xdot_scaling, True) + NLP.add(self, "u_scaling", u_scaling, True) + NLP.add(self, "s_scaling", s_scaling, True) + + # set to False because it's not relevant for traditional OCP, only relevant for StochasticOptimalControlProgram + NLP.add(self, "is_stochastic", False, True) + self._prepare_node_mapping(node_mappings) self._prepare_dynamics() self._prepare_bounds_and_init( @@ -368,6 +369,10 @@ def _check_bioptim_version(self): return def _initialize_model(self, bio_model): + """ + Initialize the bioptim model and check if the quaternions are used, if yes then setting them. + Setting the number of phases. + """ if not isinstance(bio_model, (list, tuple)): bio_model = [bio_model] bio_model = self._check_quaternions_hasattr(bio_model) @@ -458,22 +463,61 @@ def _set_original_values( } return + def _check_and_set_threads(self, n_threads): + if not isinstance(n_threads, int) or isinstance(n_threads, bool) or n_threads < 1: + raise RuntimeError("n_threads should be a positive integer greater or equal than 1") + self.n_threads = n_threads + + def _check_and_set_shooting_points(self, n_shooting): + if not isinstance(n_shooting, int) or n_shooting < 2: + if isinstance(n_shooting, (tuple, list)): + if sum([True for i in n_shooting if not isinstance(i, int) and not isinstance(i, bool)]) != 0: + raise RuntimeError("n_shooting should be a positive integer (or a list of) greater or equal than 2") + else: + raise RuntimeError("n_shooting should be a positive integer (or a list of) greater or equal than 2") + self.n_shooting = n_shooting + + def _check_and_set_phase_time(self, phase_time): + if not isinstance(phase_time, (int, float)): + if isinstance(phase_time, (tuple, list)): + if sum([True for i in phase_time if not isinstance(i, (int, float))]) != 0: + raise RuntimeError("phase_time should be a number or a list of number") + else: + raise RuntimeError("phase_time should be a number or a list of number") + self.phase_time = phase_time + + def _check_and_prepare_decision_variables( + self, + var_name: str, + bounds: BoundsList, + init: InitialGuessList, + scaling: VariableScalingList, + ): + """ + This function checks if the decision variables are of the right type for initial guess and bounds. + It also prepares the scaling for the decision variables. + And set them in a dictionary for the phase. + """ + + if bounds is None: + bounds = BoundsList() + elif not isinstance(bounds, BoundsList): + raise RuntimeError(f"{var_name}_bounds should be built from a BoundsList") + + if init is None: + init = InitialGuessList() + elif not isinstance(init, InitialGuessList): + raise RuntimeError(f"{var_name}_init should be built from a InitialGuessList") + + bounds = self._prepare_option_dict_for_phase(f"{var_name}_bounds", bounds, BoundsList) + init = self._prepare_option_dict_for_phase(f"{var_name}_init", init, InitialGuessList) + scaling = self._prepare_option_dict_for_phase(f"{var_name}_scaling", scaling, VariableScalingList) + + return bounds, init, scaling + def _check_arguments_and_build_nlp( self, dynamics, - n_threads, - n_shooting, - phase_time, - x_bounds, - u_bounds, - s_bounds, - x_init, - u_init, - s_init, - x_scaling, - xdot_scaling, - u_scaling, - s_scaling, objective_functions, constraints, parameters, @@ -495,66 +539,6 @@ def _check_arguments_and_build_nlp( variable_mappings, integrated_value_functions, ): - if not isinstance(n_threads, int) or isinstance(n_threads, bool) or n_threads < 1: - raise RuntimeError("n_threads should be a positive integer greater or equal than 1") - - ns = n_shooting - if not isinstance(ns, int) or ns < 2: - if isinstance(ns, (tuple, list)): - if sum([True for i in ns if not isinstance(i, int) and not isinstance(i, bool)]) != 0: - raise RuntimeError("n_shooting should be a positive integer (or a list of) greater or equal than 2") - else: - raise RuntimeError("n_shooting should be a positive integer (or a list of) greater or equal than 2") - - if not isinstance(phase_time, (int, float)): - if isinstance(phase_time, (tuple, list)): - if sum([True for i in phase_time if not isinstance(i, (int, float))]) != 0: - raise RuntimeError("phase_time should be a number or a list of number") - else: - raise RuntimeError("phase_time should be a number or a list of number") - - if x_bounds is None: - x_bounds = BoundsList() - elif not isinstance(x_bounds, BoundsList): - raise RuntimeError("x_bounds should be built from a BoundsList") - - if u_bounds is None: - u_bounds = BoundsList() - elif not isinstance(u_bounds, BoundsList): - raise RuntimeError("u_bounds should be built from a BoundsList") - - if s_bounds is None: - s_bounds = BoundsList() - elif not isinstance(s_bounds, BoundsList): - raise RuntimeError("s_bounds should be built from a BoundsList") - - if x_init is None: - x_init = InitialGuessList() - if not isinstance(x_init, InitialGuessList): - raise RuntimeError("x_init should be built from a InitialGuessList") - - if u_init is None: - u_init = InitialGuessList() - if not isinstance(u_init, InitialGuessList): - raise RuntimeError("u_init should be built from a InitialGuessList") - - if s_init is None: - s_init = InitialGuessList() - if not isinstance(s_init, InitialGuessList): - raise RuntimeError("s_init should be built from a InitialGuessList") - - x_bounds = self._prepare_option_dict_for_phase("x_bounds", x_bounds, BoundsList) - u_bounds = self._prepare_option_dict_for_phase("u_bounds", u_bounds, BoundsList) - s_bounds = self._prepare_option_dict_for_phase("s_bounds", s_bounds, BoundsList) - - x_init = self._prepare_option_dict_for_phase("x_init", x_init, InitialGuessList) - u_init = self._prepare_option_dict_for_phase("u_init", u_init, InitialGuessList) - s_init = self._prepare_option_dict_for_phase("s_init", s_init, InitialGuessList) - - x_scaling = self._prepare_option_dict_for_phase("x_scaling", x_scaling, VariableScalingList) - xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) - u_scaling = self._prepare_option_dict_for_phase("u_scaling", u_scaling, VariableScalingList) - s_scaling = self._prepare_option_dict_for_phase("s_scaling", s_scaling, VariableScalingList) if objective_functions is None: objective_functions = ObjectiveList() @@ -632,7 +616,7 @@ def _check_arguments_and_build_nlp( if not isinstance(use_sx, bool): raise RuntimeError("use_sx should be a bool") - if not assume_phase_dynamics and n_threads > 1: + if not assume_phase_dynamics and self.n_threads > 1: raise RuntimeError("n_threads is greater than 1 is not compatible with assume_phase_dynamics=False") # Type of CasADi graph @@ -655,17 +639,17 @@ def _check_arguments_and_build_nlp( NLP.add(self, "phase_idx", [i for i in range(self.n_phases)], False) # Define some aliases - NLP.add(self, "ns", n_shooting, False) + NLP.add(self, "ns", self.n_shooting, False) for nlp in self.nlp: if nlp.ns < 1: raise RuntimeError("Number of shooting points must be at least 1") - self.n_threads = n_threads - NLP.add(self, "n_threads", n_threads, True) + NLP.add(self, "n_threads", self.n_threads, True) self.ocp_solver = None self.is_warm_starting = False # External forces + # Todo: it should be placed in the dynamics, it does not make sense to have it here anymore if external_forces is not None: NLP.add(self, "external_forces", external_forces, False) @@ -689,7 +673,7 @@ def _check_arguments_and_build_nlp( self.time_phase_mapping = time_phase_mapping # Add any time related parameters to the parameters list before declaring it - self._define_time(phase_time, objective_functions, constraints, parameters, parameter_init, parameter_bounds) + self._define_time(self.phase_time, objective_functions, constraints, parameters, parameter_init, parameter_bounds) # Declare and fill the parameters self.parameters = ParameterList() @@ -707,14 +691,8 @@ def _check_arguments_and_build_nlp( variable_mappings = variable_mappings.variable_mapping_fill_phases(self.n_phases) NLP.add(self, "variable_mappings", variable_mappings, True) - # Do not copy singleton since x_scaling was already dealt with before - NLP.add(self, "x_scaling", x_scaling, True) - NLP.add(self, "xdot_scaling", xdot_scaling, True) - NLP.add(self, "u_scaling", u_scaling, True) - NLP.add(self, "s_scaling", s_scaling, True) - NLP.add(self, "integrated_value_functions", integrated_value_functions, True) - NLP.add(self, "is_stochastic", False, True) + return ( constraints, objective_functions, @@ -723,14 +701,8 @@ def _check_arguments_and_build_nlp( multinode_constraints, multinode_objectives, phase_transitions, - x_bounds, - u_bounds, parameter_bounds, - s_bounds, - x_init, - u_init, parameter_init, - s_init, ) def _prepare_node_mapping(self, node_mappings): diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index 87803df98..8317e2b63 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -151,6 +151,16 @@ def __init__( integrated_value_functions, ) + self._check_and_set_threads(n_threads) + self._check_and_set_shooting_points(n_shooting) + self._check_and_set_phase_time(phase_time) + + x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling) + u_bounds, u_init, u_scaling = self._check_and_prepare_decision_variables("u", u_bounds, u_init, u_scaling) + s_bounds, s_init, s_scaling = self._check_and_prepare_decision_variables("s", s_bounds, s_init, s_scaling) + + xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) + ( constraints, objective_functions, @@ -159,29 +169,10 @@ def __init__( multinode_constraints, multinode_objectives, phase_transitions, - x_bounds, - u_bounds, parameter_bounds, - s_bounds, - x_init, - u_init, parameter_init, - s_init, ) = self._check_arguments_and_build_nlp( dynamics, - n_threads, - n_shooting, - phase_time, - x_bounds, - u_bounds, - s_bounds, - x_init, - u_init, - s_init, - x_scaling, - xdot_scaling, - u_scaling, - s_scaling, objective_functions, constraints, parameters, @@ -203,8 +194,16 @@ def __init__( variable_mappings, integrated_value_functions, ) + + # Do not copy singleton since x_scaling was already dealt with before + NLP.add(self, "x_scaling", x_scaling, True) + NLP.add(self, "xdot_scaling", xdot_scaling, True) + NLP.add(self, "u_scaling", u_scaling, True) + NLP.add(self, "s_scaling", s_scaling, True) + self.problem_type = problem_type NLP.add(self, "is_stochastic", True, True) + self._prepare_node_mapping(node_mappings) self._prepare_dynamics() self._prepare_bounds_and_init( From 398cec84ec08bb013b65cd1bb511189f6d4f7453 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 12:08:37 -0400 Subject: [PATCH 02/18] removing them from ocp --- .../optimization/optimal_control_program.py | 27 +++++-------------- 1 file changed, 7 insertions(+), 20 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 48adbf4d7..fc7320068 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -149,10 +149,8 @@ def __init__( phase_time: int | float | list | tuple, x_bounds: BoundsList = None, u_bounds: BoundsList = None, - s_bounds: BoundsList = None, x_init: InitialGuessList | None = None, u_init: InitialGuessList | None = None, - s_init: InitialGuessList | None = None, objective_functions: Objective | ObjectiveList = None, constraints: Constraint | ConstraintList = None, parameters: ParameterList = None, @@ -173,7 +171,6 @@ def __init__( x_scaling: VariableScalingList = None, xdot_scaling: VariableScalingList = None, u_scaling: VariableScalingList = None, - s_scaling: VariableScalingList = None, state_continuity_weight: float = None, # TODO: docstring n_threads: int = 1, use_sx: bool = False, @@ -196,22 +193,16 @@ def __init__( The initial guesses for the states u_init: InitialGuess | InitialGuessList The initial guesses for the controls - s_init: InitialGuess | InitialGuessList - The initial guesses for the stochastic variables x_bounds: Bounds | BoundsList The bounds for the states u_bounds: Bounds | BoundsList The bounds for the controls - s_bounds: Bounds | BoundsList - The bounds for the stochastic variables x_scaling: VariableScalingList The scaling for the states at each phase, if only one is sent, then the scaling is copied over the phases xdot_scaling: VariableScalingList The scaling for the states derivative, if only one is sent, then the scaling is copied over the phases u_scaling: VariableScalingList The scaling for the controls, if only one is sent, then the scaling is copied over the phases - s_scaling: VariableScalingList - The scaling for the stochastic variables, if only one is sent, then the scaling is copied over the phases objective_functions: Objective | ObjectiveList All the objective function of the program constraints: Constraint | ConstraintList @@ -257,21 +248,24 @@ def __init__( bio_model = self._initialize_model(bio_model) + # s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram + s_init = InitialGuessList() + s_bounds = BoundsList() + s_scaling = VariableScalingList() + # Placed here because of MHE + self._check_and_prepare_dynamics(dynamics) + self._set_original_values( bio_model, - dynamics, n_shooting, phase_time, x_init, u_init, - s_init, x_bounds, u_bounds, - s_bounds, x_scaling, xdot_scaling, u_scaling, - s_scaling, external_forces, ode_solver, control_type, @@ -382,19 +376,15 @@ def _initialize_model(self, bio_model): def _set_original_values( self, bio_model, - dynamics, n_shooting, phase_time, x_init, u_init, - s_init, x_bounds, u_bounds, - s_bounds, x_scaling, xdot_scaling, u_scaling, - s_scaling, external_forces, ode_solver, control_type, @@ -430,14 +420,11 @@ def _set_original_values( "phase_time": phase_time, "x_init": x_init, "u_init": u_init, - "s_init": s_init, "x_bounds": x_bounds, "u_bounds": u_bounds, - "s_bounds": s_bounds, "x_scaling": x_scaling, "xdot_scaling": xdot_scaling, "u_scaling": u_scaling, - "s_scaling": s_scaling, "objective_functions": ObjectiveList(), "constraints": ConstraintList(), "parameters": ParameterList(), From 7e06b2e6cb3fb8014a3d8c4534c18d04150f64a2 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 12:10:11 -0400 Subject: [PATCH 03/18] extra safe to original values --- .../stochastic_optimal_control_program.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index 8317e2b63..3eb590ca9 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -115,21 +115,20 @@ def __init__( else: raise ValueError("Wrong choice of ode_solver") + # Placed here because of MHE + self._check_and_prepare_dynamics(dynamics) + self._set_original_values( bio_model, - dynamics, n_shooting, phase_time, x_init, u_init, - s_init, x_bounds, u_bounds, - s_bounds, x_scaling, xdot_scaling, u_scaling, - s_scaling, external_forces, ode_solver, control_type, @@ -150,6 +149,7 @@ def __init__( assume_phase_dynamics, integrated_value_functions, ) + self._set_stochastic_variables_to_original_values(s_init, s_bounds, s_scaling) self._check_and_set_threads(n_threads) self._check_and_set_shooting_points(n_shooting) @@ -362,3 +362,13 @@ def _prepare_stochastic_dynamics_collocation(self, constraints): pt.name = f"COVARIANCE_PHASE_TRANSITION ({pt.type.name}) {pt.nodes_phase[0] % self.n_phases}->{pt.nodes_phase[1] % self.n_phases}" pt.list_index = -1 pt.add_or_replace_to_penalty_pool(self, self.nlp[pt.nodes_phase[0]]) + + def _set_stochastic_variables_to_original_values( + self, + s_init: InitialGuessList, + s_bounds: BoundsList, + s_scaling: VariableScalingList, + ): + self.original_values["s_init"] = s_init + self.original_values["s_bounds"] = s_bounds + self.original_values["s_scaling"] = s_scaling From 1281d11b9cd6ca6235ab487b63f4c01a8ee91880 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 12:10:32 -0400 Subject: [PATCH 04/18] black --- bioptim/optimization/optimal_control_program.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index fc7320068..5fbf63562 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -474,11 +474,11 @@ def _check_and_set_phase_time(self, phase_time): self.phase_time = phase_time def _check_and_prepare_decision_variables( - self, - var_name: str, - bounds: BoundsList, - init: InitialGuessList, - scaling: VariableScalingList, + self, + var_name: str, + bounds: BoundsList, + init: InitialGuessList, + scaling: VariableScalingList, ): """ This function checks if the decision variables are of the right type for initial guess and bounds. @@ -526,7 +526,6 @@ def _check_arguments_and_build_nlp( variable_mappings, integrated_value_functions, ): - if objective_functions is None: objective_functions = ObjectiveList() elif isinstance(objective_functions, Objective): @@ -660,7 +659,9 @@ def _check_arguments_and_build_nlp( self.time_phase_mapping = time_phase_mapping # Add any time related parameters to the parameters list before declaring it - self._define_time(self.phase_time, objective_functions, constraints, parameters, parameter_init, parameter_bounds) + self._define_time( + self.phase_time, objective_functions, constraints, parameters, parameter_init, parameter_bounds + ) # Declare and fill the parameters self.parameters = ParameterList() From 57ad2227458aae9325710e2c5b21e9c5f9f36ae5 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 12:10:52 -0400 Subject: [PATCH 05/18] dynamics before original values for mhe --- .../optimization/optimal_control_program.py | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 5fbf63562..8e3b930eb 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -373,6 +373,16 @@ def _initialize_model(self, bio_model): self.n_phases = len(bio_model) return bio_model + def _check_and_prepare_dynamics(self, dynamics): + if isinstance(dynamics, Dynamics): + dynamics_type_tp = DynamicsList() + dynamics_type_tp.add(dynamics) + self.dynamics = dynamics_type_tp + elif isinstance(dynamics, DynamicsList): + self.dynamics = dynamics + elif not isinstance(dynamics, DynamicsList): + raise RuntimeError("dynamics should be a Dynamics or a DynamicsList") + def _set_original_values( self, bio_model, @@ -405,17 +415,9 @@ def _set_original_values( assume_phase_dynamics, integrated_value_functions, ): - # Placed here because of MHE - if isinstance(dynamics, Dynamics): - dynamics_type_tp = DynamicsList() - dynamics_type_tp.add(dynamics) - dynamics = dynamics_type_tp - elif not isinstance(dynamics, DynamicsList): - raise RuntimeError("dynamics should be a Dynamics or a DynamicsList") - self.original_values = { "bio_model": [m.serialize() for m in bio_model], - "dynamics": dynamics, + "dynamics": self.dynamics, "n_shooting": n_shooting, "phase_time": phase_time, "x_init": x_init, From 3793e0e0e6b9dcdcd29dc8ac85f0250a37609851 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 12:11:09 -0400 Subject: [PATCH 06/18] loading for stochastic optimal control --- .../stochastic_optimal_control_program.py | 48 ++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index 3eb590ca9..e1be36875 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -1,4 +1,7 @@ from typing import Callable, Any +import sys + +import pickle from .non_linear_program import NonLinearProgram as NLP from ..dynamics.configure_problem import DynamicsList, Dynamics @@ -17,11 +20,14 @@ from ..limits.objective_functions import ObjectiveList, Objective, ParameterObjectiveList from ..limits.path_conditions import BoundsList from ..limits.path_conditions import InitialGuessList +from ..misc.__version__ import __version__ from ..misc.enums import Node, ControlType from ..misc.mapping import BiMappingList, Mapping, NodeMappingList, BiMapping +from ..misc.utils import check_version +from ..optimization.optimal_control_program import OptimalControlProgram from ..optimization.parameters import ParameterList from ..optimization.problem_type import SocpType -from ..optimization.optimal_control_program import OptimalControlProgram +from ..optimization.solution import Solution from ..optimization.variable_scaling import VariableScalingList @@ -372,3 +378,43 @@ def _set_stochastic_variables_to_original_values( self.original_values["s_init"] = s_init self.original_values["s_bounds"] = s_bounds self.original_values["s_scaling"] = s_scaling + + @staticmethod + def load(file_path: str) -> list: + """ + Reload a previous optimization (*.bo) saved using save + + Parameters + ---------- + file_path: str + The path to the *.bo file + + Returns + ------- + The ocp and sol structure. If it was saved, the iterations are also loaded + """ + + with open(file_path, "rb") as file: + try: + data = pickle.load(file) + except BaseException as error_message: + raise ValueError( + f"The file '{file_path}' cannot be loaded, maybe the version of bioptim (version {__version__})\n" + f"is not the same as the one that created the file (version unknown). For more information\n" + "please refer to the original error message below\n\n" + f"{type(error_message).__name__}: {error_message}" + ) + ocp = StochasticOptimalControlProgram.from_loaded_data(data["ocp_initializer"]) + for key in data["versions"].keys(): + key_module = "biorbd_casadi" if key == "biorbd" else key + try: + check_version(sys.modules[key_module], data["versions"][key], ocp.version[key], exclude_max=False) + except ImportError: + raise ImportError( + f"Version of {key} from file ({data['versions'][key]}) is not the same as the " + f"installed version ({ocp.version[key]})" + ) + sol = data["sol"] + sol.ocp = Solution.SimplifiedOCP(ocp) + out = [ocp, sol] + return out \ No newline at end of file From 60325871e2b0e2c490142986d7f854dc62086388 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 12:24:58 -0400 Subject: [PATCH 07/18] extra space --- bioptim/optimization/optimal_control_program.py | 1 + bioptim/optimization/stochastic_optimal_control_program.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 8e3b930eb..4a41590de 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -252,6 +252,7 @@ def __init__( s_init = InitialGuessList() s_bounds = BoundsList() s_scaling = VariableScalingList() + # Placed here because of MHE self._check_and_prepare_dynamics(dynamics) diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index e1be36875..a9769f118 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -417,4 +417,4 @@ def load(file_path: str) -> list: sol = data["sol"] sol.ocp = Solution.SimplifiedOCP(ocp) out = [ocp, sol] - return out \ No newline at end of file + return out From d2c8452cd107e954d95c78ffecf445f37d7d5fa9 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 25 Sep 2023 13:20:10 -0400 Subject: [PATCH 08/18] Getting rid of s_init in mhe and nmpc but still need to give it to solution. --- .../receding_horizon_optimization.py | 22 +++++++------------ 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/bioptim/optimization/receding_horizon_optimization.py b/bioptim/optimization/receding_horizon_optimization.py index ee9a3b9ba..fd5447b1e 100644 --- a/bioptim/optimization/receding_horizon_optimization.py +++ b/bioptim/optimization/receding_horizon_optimization.py @@ -220,8 +220,6 @@ def _initialize_solution(self, states: list, controls: list): controls_tp = controls_tp[:, :-1] u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME) - s_init = InitialGuessList() - p_init = InitialGuessList() model_class = self.original_values["bio_model"][0][0] model_initializer = self.original_values["bio_model"][0][1] solution_ocp = OptimalControlProgram( @@ -235,10 +233,10 @@ def _initialize_solution(self, states: list, controls: list): u_bounds=self.nlp[0].u_bounds, x_init=x_init, u_init=u_init, - s_init=s_init, use_sx=self.original_values["use_sx"], ) - + s_init = InitialGuessList() + p_init = InitialGuessList() return Solution(solution_ocp, [x_init, u_init_for_solution, p_init, s_init]) def advance_window(self, sol: Solution, steps: int = 0, **advance_options): @@ -444,9 +442,6 @@ def _initialize_solution(self, states: list, controls: list): if self.original_values["control_type"] == ControlType.CONSTANT: controls_tp = controls_tp[:, :-1] u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME) - - s_init = InitialGuessList() - p_init = InitialGuessList() model_class = self.original_values["bio_model"][0][0] model_initializer = self.original_values["bio_model"][0][1] solution_ocp = OptimalControlProgram( @@ -458,10 +453,11 @@ def _initialize_solution(self, states: list, controls: list): u_bounds=self.nlp[0].u_bounds, x_init=x_init, u_init=u_init, - s_init=s_init, skip_continuity=True, use_sx=self.original_values["use_sx"], ) + s_init = InitialGuessList() + p_init = InitialGuessList() return Solution(solution_ocp, [x_init, u_init_for_solution, p_init, s_init]) def _initialize_state_idx_to_cycle(self, options): @@ -682,8 +678,6 @@ def _initialize_solution(self, states: list, controls: list): controls_tp = controls_tp[:, :-1] u_init.add(key, controls_tp, interpolation=InterpolationType.EACH_FRAME, phase=0) - s_init = InitialGuessList() - p_init = InitialGuessList() model_class = self.original_values["bio_model"][0][0] model_initializer = self.original_values["bio_model"][0][1] solution_ocp = OptimalControlProgram( @@ -696,9 +690,10 @@ def _initialize_solution(self, states: list, controls: list): skip_continuity=True, x_init=x_init, u_init=u_init, - s_init=s_init, use_sx=self.original_values["use_sx"], ) + s_init = InitialGuessList() + p_init = InitialGuessList() return Solution(solution_ocp, [x_init, u_init_for_solution, p_init, s_init]) def _initialize_one_cycle(self, states: np.ndarray, controls: np.ndarray): @@ -726,8 +721,6 @@ def _initialize_one_cycle(self, states: np.ndarray, controls: np.ndarray): model_class = original_values["bio_model"][0][0] model_initializer = original_values["bio_model"][0][1] - s_init = InitialGuessList() - p_init = InitialGuessList() solution_ocp = OptimalControlProgram( bio_model=model_class(**model_initializer), dynamics=original_values["dynamics"][0], @@ -738,9 +731,10 @@ def _initialize_one_cycle(self, states: np.ndarray, controls: np.ndarray): skip_continuity=True, x_init=x_init, u_init=u_init, - s_init=s_init, use_sx=original_values["use_sx"], ) + s_init = InitialGuessList() + p_init = InitialGuessList() return Solution(solution_ocp, [x_init, u_init_for_solution, p_init, s_init]) From b3a1e6c18acee3d3809f269027029bdcd7c0f93e Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 16:54:22 -0400 Subject: [PATCH 09/18] resolve conflicts again --- bioptim/optimization/optimal_control_program.py | 2 -- bioptim/optimization/receding_horizon_optimization.py | 1 - 2 files changed, 3 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index f784eea42..fa26cf009 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -172,8 +172,6 @@ def __init__( x_scaling: VariableScalingList = None, xdot_scaling: VariableScalingList = None, u_scaling: VariableScalingList = None, - s_scaling: VariableScalingList = None, - state_continuity_weight: float = None, # TODO: docstring n_threads: int = 1, use_sx: bool = False, integrated_value_functions: dict[str, Callable] = None, diff --git a/bioptim/optimization/receding_horizon_optimization.py b/bioptim/optimization/receding_horizon_optimization.py index ca3760807..decb44f1c 100644 --- a/bioptim/optimization/receding_horizon_optimization.py +++ b/bioptim/optimization/receding_horizon_optimization.py @@ -452,7 +452,6 @@ def _initialize_solution(self, states: list, controls: list): u_bounds=self.nlp[0].u_bounds, x_init=x_init, u_init=u_init, - skip_continuity=True, use_sx=self.original_values["use_sx"], ) s_init = InitialGuessList() From 9dd0da0e570fb12a56e5b959e1b873edcc8cee26 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 16:54:47 -0400 Subject: [PATCH 10/18] refactoring ocp and socp first complete try --- .../optimization/optimal_control_program.py | 137 ++++++++- .../stochastic_optimal_control_program.py | 290 +++++++++--------- 2 files changed, 271 insertions(+), 156 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index fa26cf009..ea8d352a8 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -244,11 +244,6 @@ def __init__( bio_model = self._initialize_model(bio_model) - # s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram - s_init = InitialGuessList() - s_bounds = BoundsList() - s_scaling = VariableScalingList() - # Placed here because of MHE self._check_and_prepare_dynamics(dynamics) @@ -281,16 +276,36 @@ def __init__( use_sx, integrated_value_functions, ) + s_init, s_bounds, s_scaling = self.__set_stochastic_internal_stochastic_variables() + self._set_stochastic_variables_to_original_values(s_init, s_bounds, s_scaling) self._check_and_set_threads(n_threads) self._check_and_set_shooting_points(n_shooting) self._check_and_set_phase_time(phase_time) - x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling) - u_bounds, u_init, u_scaling = self._check_and_prepare_decision_variables("u", u_bounds, u_init, u_scaling) - s_bounds, s_init, s_scaling = self._check_and_prepare_decision_variables("s", s_bounds, s_init, s_scaling) - - xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) + ( + x_bounds, + x_init, + x_scaling, + u_bounds, + u_init, + u_scaling, + s_bounds, + s_init, + s_scaling, + xdot_scaling, + ) = self._prepare_all_decision_variables( + x_bounds, + x_init, + x_scaling, + u_bounds, + u_init, + u_scaling, + xdot_scaling, + s_bounds, + s_init, + s_scaling, + ) ( constraints, @@ -331,8 +346,7 @@ def __init__( NLP.add(self, "u_scaling", u_scaling, True) NLP.add(self, "s_scaling", s_scaling, True) - # set to False because it's not relevant for traditional OCP, only relevant for StochasticOptimalControlProgram - NLP.add(self, "is_stochastic", False, True) + self.__set_nlp_is_stochastic() self._prepare_node_mapping(node_mappings) self._prepare_dynamics() @@ -492,6 +506,37 @@ def _check_and_prepare_decision_variables( return bounds, init, scaling + def _prepare_all_decision_variables( + self, + x_bounds, + x_init, + x_scaling, + u_bounds, + u_init, + u_scaling, + xdot_scaling, + s_bounds, + s_init, + s_scaling, + ): + """ + This function checks if the decision variables are of the right type for initial guess and bounds. + It also prepares the scaling for the decision variables. + + Note + ---- + s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram + This method is overriden in StochasticOptimalControlProgram + """ + + x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling) + u_bounds, u_init, u_scaling = self._check_and_prepare_decision_variables("u", u_bounds, u_init, u_scaling) + s_bounds, s_init, s_scaling = self._check_and_prepare_decision_variables("s", s_bounds, s_init, s_scaling) + + xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) + + return x_bounds, u_bounds, s_bounds, x_init, u_init, s_init, x_scaling, xdot_scaling, u_scaling, s_scaling + def _check_arguments_and_build_nlp( self, dynamics, @@ -584,7 +629,7 @@ def _check_arguments_and_build_nlp( raise RuntimeError("constraints should be built from an Constraint or ConstraintList") if ode_solver is None: - ode_solver = OdeSolver.RK4() + ode_solver = self._set_default_ode_solver() elif not isinstance(ode_solver, OdeSolverBase): raise RuntimeError("ode_solver should be built an instance of OdeSolver") @@ -733,7 +778,20 @@ def _prepare_bounds_and_init( # Define the actual NLP problem OptimizationVectorHelper.declare_ocp_shooting_points(self) - def _declare_multi_node_penalties(self, multinode_constraints: ConstraintList, multinode_objectives: ObjectiveList): + def _declare_multi_node_penalties( + self, + multinode_constraints: ConstraintList, + multinode_objectives: ObjectiveList, + constraints: ConstraintList, + phase_transition: PhaseTransitionList, + ): + """ + This function declares the multi node penalties (constraints and objectives) to the penalty pool. + + Note + ---- + This function is overriden in StochasticOptimalControlProgram + """ multinode_constraints.add_or_replace_to_penalty_pool(self) multinode_objectives.add_or_replace_to_penalty_pool(self) @@ -1888,6 +1946,57 @@ def node_time(self, phase_idx: int, node_idx: int): previous_phase_time = sum([nlp.tf for nlp in self.nlp[:phase_idx]]) return previous_phase_time + self.nlp[phase_idx].node_time(node_idx) + def _set_default_ode_solver(self): + """ + Set the default ode solver to RK4 + + Note + ---- + This method is overrided in StochasticOptimalControlProgram + """ + return OdeSolver.RK4() + + def _set_stochastic_variables_to_original_values( + self, + s_init: InitialGuessList, + s_bounds: BoundsList, + s_scaling: VariableScalingList, + ): + """ + Set the stochastic variables to their original values + + Note + ---- + This method is overrided in StochasticOptimalControlProgram + """ + pass + + def __set_stochastic_internal_stochastic_variables(self): + """ + Set the stochastic variables to their internal values + + Note + ---- + This method is overrided in StochasticOptimalControlProgram + """ + # s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram + self.__s_init = InitialGuessList() + self.__s_bounds = BoundsList() + self.__s_scaling = VariableScalingList() + return self.__s_init, self.__s_bounds, self.__s_scaling + + def __set_nlp_is_stochastic(self): + """ + Set the is_stochastic variable to False + because it's not relevant for traditional OCP, + only relevant for StochasticOptimalControlProgram + + Note + ---- + This method is thus overriden in StochasticOptimalControlProgram + """ + NLP.add(self, "is_stochastic", False, True) + # helpers def _reshape_to_column(array) -> np.ndarray: diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index d75f6c8da..a94eab966 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -76,150 +76,47 @@ def __init__( problem_type=SocpType.TRAPEZOIDAL_IMPLICIT, **kwargs, ): - """ """ - - if not isinstance(problem_type, SocpType.COLLOCATION): - if "n_thread" in kwargs: - if kwargs["n_thread"] != 1: - raise ValueError( - "Multi-threading is not possible yet while solving a trapezoidal stochastic ocp." - "n_thread is set to 1 by default." - ) - self.n_threads = n_threads - - if "ode_solver" in kwargs: - raise ValueError( - "The ode_solver cannot be defined for a stochastic ocp. The value is chosen based on the type of problem solved:" - "\n- TRAPEZOIDAL_EXPLICIT: OdeSolver.TRAPEZOIDAL() " - "\n- TRAPEZOIDAL_IMPLICIT: OdeSolver.TRAPEZOIDAL() " - "\n- COLLOCATION: OdeSolver.COLLOCATION(method=problem_type.method, polynomial_degree=problem_type.polynomial_degree, include_starting_collocation_point=True)" - ) - - if not isinstance(problem_type, SocpType.COLLOCATION): - if "phase_dynamics" in kwargs: - if kwargs["phase_dynamics"] == PhaseDynamics.SHARED_DURING_THE_PHASE: - raise ValueError( - "The dynamics cannot be SHARED_DURING_THE_PHASE with a trapezoidal stochastic ocp." - "phase_dynamics is set to PhaseDynamics.ONE_PER_NODE by default." - ) - - self._check_bioptim_version() - - bio_model = self._initialize_model(bio_model) - - if isinstance(problem_type, SocpType.TRAPEZOIDAL_IMPLICIT) or isinstance( - problem_type, SocpType.TRAPEZOIDAL_EXPLICIT - ): - ode_solver = OdeSolver.TRAPEZOIDAL() - elif isinstance(problem_type, SocpType.COLLOCATION): - ode_solver = OdeSolver.COLLOCATION( - method=problem_type.method, - polynomial_degree=problem_type.polynomial_degree, - include_starting_collocation_point=True, - ) - else: - raise RuntimeError("Wrong choice of problem_type, you must choose one of the SocpType.") - - # Placed here because of MHE - self._check_and_prepare_dynamics(dynamics) - - self._set_original_values( - bio_model, - n_shooting, - phase_time, - x_init, - u_init, - x_bounds, - u_bounds, - x_scaling, - xdot_scaling, - u_scaling, - external_forces, - ode_solver, - control_type, - variable_mappings, - time_phase_mapping, - node_mappings, - plot_mappings, - phase_transitions, - multinode_constraints, - multinode_objectives, - parameter_bounds, - parameter_init, - parameter_constraints, - parameter_objectives, - n_threads, - use_sx, - integrated_value_functions, - ) - self._set_stochastic_variables_to_original_values(s_init, s_bounds, s_scaling) - - self._check_and_set_threads(n_threads) - self._check_and_set_shooting_points(n_shooting) - self._check_and_set_phase_time(phase_time) - - x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling) - u_bounds, u_init, u_scaling = self._check_and_prepare_decision_variables("u", u_bounds, u_init, u_scaling) - s_bounds, s_init, s_scaling = self._check_and_prepare_decision_variables("s", s_bounds, s_init, s_scaling) - - xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) - - ( - constraints, - objective_functions, - parameter_constraints, - parameter_objectives, - multinode_constraints, - multinode_objectives, - phase_transitions, - parameter_bounds, - parameter_init, - ) = self._check_arguments_and_build_nlp( - dynamics, - objective_functions, - constraints, - parameters, - phase_transitions, - multinode_constraints, - multinode_objectives, - parameter_bounds, - parameter_init, - parameter_constraints, - parameter_objectives, - ode_solver, - use_sx, - bio_model, - external_forces, - plot_mappings, - time_phase_mapping, - control_type, - variable_mappings, - integrated_value_functions, - ) - - # Do not copy singleton since x_scaling was already dealt with before - NLP.add(self, "x_scaling", x_scaling, True) - NLP.add(self, "xdot_scaling", xdot_scaling, True) - NLP.add(self, "u_scaling", u_scaling, True) - NLP.add(self, "s_scaling", s_scaling, True) + _check_multi_threading_and_problem_type(problem_type, **kwargs) + _check_has_no_ode_solver_defined(**kwargs) + _check_has_no_phase_dynamics_shared_during_the_phase(problem_type, **kwargs) self.problem_type = problem_type - NLP.add(self, "is_stochastic", True, True) - - self._prepare_node_mapping(node_mappings) - self._prepare_dynamics() - self._prepare_bounds_and_init( - x_bounds, u_bounds, parameter_bounds, s_bounds, x_init, u_init, parameter_init, s_init - ) - - self._declare_multi_node_penalties(multinode_constraints, multinode_objectives, constraints, phase_transitions) - - self._finalize_penalties( - constraints, - parameter_constraints, - objective_functions, - parameter_objectives, - phase_transitions, + self.__s_init = s_init + self.__s_bounds = s_bounds + self.__s_scaling = s_scaling + + super(StochasticOptimalControlProgram, self).__init__( + bio_model=bio_model, + dynamics=dynamics, + n_shooting=n_shooting, + phase_time=phase_time, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + constraints=constraints, + parameters=parameters, + parameter_bounds=parameter_bounds, + parameter_init=parameter_init, + parameter_objectives=parameter_objectives, + parameter_constraints=parameter_constraints, + external_forces=external_forces, + ode_solver=None, + control_type=control_type, + variable_mappings=variable_mappings, + time_phase_mapping=time_phase_mapping, + node_mappings=node_mappings, + plot_mappings=plot_mappings, + phase_transitions=phase_transitions, + multinode_constraints=multinode_constraints, + multinode_objectives=multinode_objectives, + x_scaling=x_scaling, + xdot_scaling=xdot_scaling, + u_scaling=u_scaling, + n_threads=n_threads, + use_sx=use_sx, + integrated_value_functions=integrated_value_functions, ) def _prepare_dynamics(self): @@ -236,6 +133,13 @@ def _declare_multi_node_penalties( constraints: ConstraintList, phase_transition: PhaseTransitionList, ): + """ + This function declares the multi node penalties (constraints and objectives) to the penalty pool. + + Note + ---- + This function overrides the method in OptimalControlProgram + """ multinode_constraints.add_or_replace_to_penalty_pool(self) multinode_objectives.add_or_replace_to_penalty_pool(self) @@ -416,3 +320,105 @@ def load(file_path: str) -> list: sol.ocp = Solution.SimplifiedOCP(ocp) out = [ocp, sol] return out + + def _set_default_ode_solver(self): + """It overrides the method in OptimalControlProgram that set a RK4 by default""" + if isinstance(self.problem_type, SocpType.TRAPEZOIDAL_IMPLICIT) or isinstance( + self.problem_type, SocpType.TRAPEZOIDAL_EXPLICIT + ): + return OdeSolver.TRAPEZOIDAL() + elif isinstance(self.problem_type, SocpType.COLLOCATION): + return OdeSolver.COLLOCATION( + method=self.problem_type.method, + polynomial_degree=self.problem_type.polynomial_degree, + include_starting_collocation_point=True, + ) + else: + raise RuntimeError("Wrong choice of problem_type, you must choose one of the SocpType.") + + def _prepare_all_decision_variables( + self, + x_bounds, + x_init, + x_scaling, + u_bounds, + u_init, + u_scaling, + xdot_scaling, + s_bounds, + s_init, + s_scaling, + ): + """ + This function checks if the decision variables are of the right type for initial guess and bounds. + It also prepares the scaling for the decision variables. + + Note + ---- + s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram + This method is overriden in StochasticOptimalControlProgram + """ + + x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling) + u_bounds, u_init, u_scaling = self._check_and_prepare_decision_variables("u", u_bounds, u_init, u_scaling) + s_bounds, s_init, s_scaling = self._check_and_prepare_decision_variables("s", s_bounds, s_init, s_scaling) + + xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) + + return x_bounds, u_bounds, s_bounds, x_init, u_init, s_init, x_scaling, xdot_scaling, u_scaling, s_scaling + + def __set_stochastic_internal_stochastic_variables(self): + """ + Set the stochastic variables to their internal values + + Note + ---- + This method overrides the method in OptimalControlProgram + """ + pass # Nothing to do here as they are already set before calling super().__init__ + + def __set_nlp_is_stochastic(self): + """ + Set the is_stochastic variable to True for all the nlp + + Note + ---- + This method overrides the method in OptimalControlProgram + """ + NLP.add(self, "is_stochastic", True, True) + + +def _check_multi_threading_and_problem_type(problem_type, **kwargs): + if not isinstance(problem_type, SocpType.COLLOCATION): + if "n_thread" in kwargs: + if kwargs["n_thread"] != 1: + raise ValueError( + "Multi-threading is not possible yet while solving a trapezoidal stochastic ocp." + "n_thread is set to 1 by default." + ) + + +def _check_has_no_ode_solver_defined(**kwargs): + if "ode_solver" in kwargs: + raise ValueError( + "The ode_solver cannot be defined for a stochastic ocp. " + "The value is chosen based on the type of problem solved:" + "\n- TRAPEZOIDAL_EXPLICIT: OdeSolver.TRAPEZOIDAL() " + "\n- TRAPEZOIDAL_IMPLICIT: OdeSolver.TRAPEZOIDAL() " + "\n- COLLOCATION: " + "OdeSolver.COLLOCATION(" + "method=problem_type.method, " + "polynomial_degree=problem_type.polynomial_degree, " + "include_starting_collocation_point=True" + ")" + ) + + +def _check_has_no_phase_dynamics_shared_during_the_phase(problem_type, **kwargs): + if not isinstance(problem_type, SocpType.COLLOCATION): + if "phase_dynamics" in kwargs: + if kwargs["phase_dynamics"] == PhaseDynamics.SHARED_DURING_THE_PHASE: + raise ValueError( + "The dynamics cannot be SHARED_DURING_THE_PHASE with a trapezoidal stochastic ocp." + "phase_dynamics is set to PhaseDynamics.ONE_PER_NODE by default." + ) From 8cfe7e5e8a2220005372b63b7cd159b1b12c6836 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 16:58:42 -0400 Subject: [PATCH 11/18] small fixes --- bioptim/optimization/optimal_control_program.py | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index ea8d352a8..35c45b613 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -284,16 +284,7 @@ def __init__( self._check_and_set_phase_time(phase_time) ( - x_bounds, - x_init, - x_scaling, - u_bounds, - u_init, - u_scaling, - s_bounds, - s_init, - s_scaling, - xdot_scaling, + x_bounds, x_init, x_scaling, u_bounds, u_init, u_scaling, s_bounds, s_init, s_scaling, xdot_scaling, ) = self._prepare_all_decision_variables( x_bounds, x_init, @@ -354,7 +345,7 @@ def __init__( x_bounds, u_bounds, parameter_bounds, s_bounds, x_init, u_init, parameter_init, s_init ) - self._declare_multi_node_penalties(multinode_constraints, multinode_objectives) + self._declare_multi_node_penalties(multinode_constraints, multinode_objectives, constraints, phase_transitions) self._finalize_penalties( constraints, @@ -535,7 +526,7 @@ def _prepare_all_decision_variables( xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) - return x_bounds, u_bounds, s_bounds, x_init, u_init, s_init, x_scaling, xdot_scaling, u_scaling, s_scaling + return x_bounds, x_init, x_scaling, u_bounds, u_init, u_scaling, s_bounds, s_init, s_scaling, xdot_scaling def _check_arguments_and_build_nlp( self, From 08c4074515a66871c884e9655340fd63f13fc94d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 16:59:06 -0400 Subject: [PATCH 12/18] black --- bioptim/optimization/optimal_control_program.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 35c45b613..d5f9fd0fd 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -284,7 +284,16 @@ def __init__( self._check_and_set_phase_time(phase_time) ( - x_bounds, x_init, x_scaling, u_bounds, u_init, u_scaling, s_bounds, s_init, s_scaling, xdot_scaling, + x_bounds, + x_init, + x_scaling, + u_bounds, + u_init, + u_scaling, + s_bounds, + s_init, + s_scaling, + xdot_scaling, ) = self._prepare_all_decision_variables( x_bounds, x_init, From e67faea09ffb6f906ed4430dece509359a7d1b08 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 17:03:37 -0400 Subject: [PATCH 13/18] unit test as a gift for all --- tests/shard5/test_helper_socp.py | 52 ++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 tests/shard5/test_helper_socp.py diff --git a/tests/shard5/test_helper_socp.py b/tests/shard5/test_helper_socp.py new file mode 100644 index 000000000..224083719 --- /dev/null +++ b/tests/shard5/test_helper_socp.py @@ -0,0 +1,52 @@ +import pytest + +from bioptim import SocpType, PhaseDynamics +from bioptim.optimization.stochastic_optimal_control_program import ( + _check_multi_threading_and_problem_type, + _check_has_no_ode_solver_defined, + _check_has_no_phase_dynamics_shared_during_the_phase, +) + + +# Your functions would come here... + +# Tests for _check_multi_threading_and_problem_type() +def test_check_multi_threading_and_problem_type_no_n_thread(): + _check_multi_threading_and_problem_type("some_type") + + +def test_check_multi_threading_and_problem_type_n_thread_1(): + _check_multi_threading_and_problem_type("some_type", n_thread=1) + + +def test_check_multi_threading_and_problem_type_n_thread_not_1(): + with pytest.raises(ValueError, match="Multi-threading is not possible yet"): + _check_multi_threading_and_problem_type("some_type", n_thread=2) + + +def test_check_multi_threading_and_problem_type_collocation(): + _check_multi_threading_and_problem_type(SocpType.COLLOCATION) + + +# Tests for _check_has_no_ode_solver_defined() +def test_check_has_no_ode_solver_defined_with_ode_solver(): + with pytest.raises(ValueError, match="The ode_solver cannot be defined for a stochastic ocp"): + _check_has_no_ode_solver_defined(ode_solver="some_solver") + + +def test_check_has_no_ode_solver_defined_without_ode_solver(): + _check_has_no_ode_solver_defined() + + +# Tests for _check_has_no_phase_dynamics_shared_during_the_phase() +def test_check_has_no_phase_dynamics_shared_during_the_phase_shared_dynamics(): + with pytest.raises(ValueError, match="The dynamics cannot be SHARED_DURING_THE_PHASE"): + _check_has_no_phase_dynamics_shared_during_the_phase("some_type", phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE) + + +def test_check_has_no_phase_dynamics_shared_during_the_phase_not_shared_dynamics(): + _check_has_no_phase_dynamics_shared_during_the_phase("some_type", phase_dynamics=PhaseDynamics.ONE_PER_NODE) + + +def test_check_has_no_phase_dynamics_shared_during_the_phase_collocation(): + _check_has_no_phase_dynamics_shared_during_the_phase(SocpType.COLLOCATION) From 2b8acf44db243380e982498d7a57f72cf25d3aad Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 17:04:44 -0400 Subject: [PATCH 14/18] remove some comments. --- tests/shard5/test_helper_socp.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/tests/shard5/test_helper_socp.py b/tests/shard5/test_helper_socp.py index 224083719..8891b6590 100644 --- a/tests/shard5/test_helper_socp.py +++ b/tests/shard5/test_helper_socp.py @@ -8,8 +8,6 @@ ) -# Your functions would come here... - # Tests for _check_multi_threading_and_problem_type() def test_check_multi_threading_and_problem_type_no_n_thread(): _check_multi_threading_and_problem_type("some_type") From e6f711ebef508968af7defa6d6f89d52b7b9cda7 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 17:07:26 -0400 Subject: [PATCH 15/18] black test --- tests/shard5/test_helper_socp.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tests/shard5/test_helper_socp.py b/tests/shard5/test_helper_socp.py index 8891b6590..bc0eda5d5 100644 --- a/tests/shard5/test_helper_socp.py +++ b/tests/shard5/test_helper_socp.py @@ -39,7 +39,9 @@ def test_check_has_no_ode_solver_defined_without_ode_solver(): # Tests for _check_has_no_phase_dynamics_shared_during_the_phase() def test_check_has_no_phase_dynamics_shared_during_the_phase_shared_dynamics(): with pytest.raises(ValueError, match="The dynamics cannot be SHARED_DURING_THE_PHASE"): - _check_has_no_phase_dynamics_shared_during_the_phase("some_type", phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE) + _check_has_no_phase_dynamics_shared_during_the_phase( + "some_type", phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE + ) def test_check_has_no_phase_dynamics_shared_during_the_phase_not_shared_dynamics(): From 76fc97b930be753106e783ec11c3145212de29cf Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 17:30:15 -0400 Subject: [PATCH 16/18] fix bugs and black --- .../optimization/optimal_control_program.py | 28 +++++++--- .../stochastic_optimal_control_program.py | 54 ++++--------------- 2 files changed, 30 insertions(+), 52 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index d5f9fd0fd..d5969cb71 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -138,6 +138,18 @@ class OptimalControlProgram: __modify_penalty(self, new_penalty: PenaltyOption | Parameter) The internal function to modify a penalty. It is also stored in the original_values, meaning that if one overrides an objective only the latter is preserved when saved + __set_nlp_is_stochastic(self) + Set the nlp as stochastic if any of the phases is stochastic + __set_stochastic_internal_stochastic_variables(self) + Set the internal stochastic variables (s_init, s_bounds, s_scaling) if any of the phases is stochastic + _set_stochastic_variables_to_original_values(self, s_init, s_bounds, s_scaling) + Set the original_values with the stochastic variables (s_init, s_bounds, s_scaling) if any of the phases is + stochastic + _check_quaternions_hasattr(self, bio_model) + Check if the bio_model has quaternions and set the flag accordingly + _check_and_prepare_dynamics(self, dynamics) + Check if the dynamics is a Dynamics or a DynamicsList and set the flag accordingly + _set_original_values( """ # TODO: OCP should not be aware of s (s_init, s_bounds...) @@ -276,7 +288,7 @@ def __init__( use_sx, integrated_value_functions, ) - s_init, s_bounds, s_scaling = self.__set_stochastic_internal_stochastic_variables() + s_init, s_bounds, s_scaling = self._set_stochastic_internal_stochastic_variables() self._set_stochastic_variables_to_original_values(s_init, s_bounds, s_scaling) self._check_and_set_threads(n_threads) @@ -346,7 +358,7 @@ def __init__( NLP.add(self, "u_scaling", u_scaling, True) NLP.add(self, "s_scaling", s_scaling, True) - self.__set_nlp_is_stochastic() + self._set_nlp_is_stochastic() self._prepare_node_mapping(node_mappings) self._prepare_dynamics() @@ -1971,7 +1983,7 @@ def _set_stochastic_variables_to_original_values( """ pass - def __set_stochastic_internal_stochastic_variables(self): + def _set_stochastic_internal_stochastic_variables(self): """ Set the stochastic variables to their internal values @@ -1980,12 +1992,12 @@ def __set_stochastic_internal_stochastic_variables(self): This method is overrided in StochasticOptimalControlProgram """ # s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram - self.__s_init = InitialGuessList() - self.__s_bounds = BoundsList() - self.__s_scaling = VariableScalingList() - return self.__s_init, self.__s_bounds, self.__s_scaling + self._s_init = InitialGuessList() + self._s_bounds = BoundsList() + self._s_scaling = VariableScalingList() + return self._s_init, self._s_bounds, self._s_scaling - def __set_nlp_is_stochastic(self): + def _set_nlp_is_stochastic(self): """ Set the is_stochastic variable to False because it's not relevant for traditional OCP, diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index a94eab966..2837d278e 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -81,9 +81,9 @@ def __init__( _check_has_no_phase_dynamics_shared_during_the_phase(problem_type, **kwargs) self.problem_type = problem_type - self.__s_init = s_init - self.__s_bounds = s_bounds - self.__s_scaling = s_scaling + self._s_init = s_init + self._s_bounds = s_bounds + self._s_scaling = s_scaling super(StochasticOptimalControlProgram, self).__init__( bio_model=bio_model, @@ -119,13 +119,6 @@ def __init__( integrated_value_functions=integrated_value_functions, ) - def _prepare_dynamics(self): - # Prepare the dynamics - for i in range(self.n_phases): - self.nlp[i].initialize(self.cx) - ConfigureProblem.initialize(self, self.nlp[i]) - self.nlp[i].ode_solver.prepare_dynamic_integrator(self, self.nlp[i]) - def _declare_multi_node_penalties( self, multinode_constraints: ConstraintList, @@ -336,38 +329,7 @@ def _set_default_ode_solver(self): else: raise RuntimeError("Wrong choice of problem_type, you must choose one of the SocpType.") - def _prepare_all_decision_variables( - self, - x_bounds, - x_init, - x_scaling, - u_bounds, - u_init, - u_scaling, - xdot_scaling, - s_bounds, - s_init, - s_scaling, - ): - """ - This function checks if the decision variables are of the right type for initial guess and bounds. - It also prepares the scaling for the decision variables. - - Note - ---- - s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram - This method is overriden in StochasticOptimalControlProgram - """ - - x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling) - u_bounds, u_init, u_scaling = self._check_and_prepare_decision_variables("u", u_bounds, u_init, u_scaling) - s_bounds, s_init, s_scaling = self._check_and_prepare_decision_variables("s", s_bounds, s_init, s_scaling) - - xdot_scaling = self._prepare_option_dict_for_phase("xdot_scaling", xdot_scaling, VariableScalingList) - - return x_bounds, u_bounds, s_bounds, x_init, u_init, s_init, x_scaling, xdot_scaling, u_scaling, s_scaling - - def __set_stochastic_internal_stochastic_variables(self): + def _set_stochastic_internal_stochastic_variables(self): """ Set the stochastic variables to their internal values @@ -375,9 +337,13 @@ def __set_stochastic_internal_stochastic_variables(self): ---- This method overrides the method in OptimalControlProgram """ - pass # Nothing to do here as they are already set before calling super().__init__ + return ( + self._s_init, + self._s_bounds, + self._s_scaling, + ) # Nothing to do here as they are already set before calling super().__init__ - def __set_nlp_is_stochastic(self): + def _set_nlp_is_stochastic(self): """ Set the is_stochastic variable to True for all the nlp From ff7cfafaf7de9713ddbdc49d509516c90668a364 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 17:31:49 -0400 Subject: [PATCH 17/18] remove a todo because done ! --- bioptim/optimization/optimal_control_program.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index d5969cb71..c2fa26751 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -152,8 +152,6 @@ class OptimalControlProgram: _set_original_values( """ - # TODO: OCP should not be aware of s (s_init, s_bounds...) - def __init__( self, bio_model: list | tuple | BioModel, From 2918a294c4f315794384089e9a529ce6094274e1 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 16 Oct 2023 17:49:22 -0400 Subject: [PATCH 18/18] some comments and docstrings adjustements --- bioptim/dynamics/dynamics_functions.py | 3 +++ bioptim/optimization/optimal_control_program.py | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index ef958fc43..87d971565 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -235,6 +235,9 @@ def stochastic_torque_driven( sensory_input = nlp.model.sensory_reference(states, controls, parameters, stochastic_variables, nlp) + # TODO: Most of the equations should be displaced in the StochasticBiorbdModel + # - compute_sensory_feedback_torque(k, ref, sensory_input) + # - compute_torques_from_motor_noise_and_sensory_feedback(k, ref, sensory_input) mapped_motor_noise = nlp.model.motor_noise_sym mapped_sensory_feedback_torque = k_matrix @ ((sensory_input - ref) + nlp.model.sensory_noise_sym) if "tau" in nlp.model.motor_noise_mapping.keys(): diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index c2fa26751..52fd17b7d 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -536,7 +536,6 @@ def _prepare_all_decision_variables( Note ---- s decision variables are not relevant for traditional OCPs, only relevant for StochasticOptimalControlProgram - This method is overriden in StochasticOptimalControlProgram """ x_bounds, x_init, x_scaling = self._check_and_prepare_decision_variables("x", x_bounds, x_init, x_scaling)