Merge pull request #684 from pariterre/fix_scaling

Fix scaling

bioptim/__init__.py

@@ -174,18 +174,12 @@
 from .interfaces.solver_options import Solver
 from .interfaces.biorbd_model import BiorbdModel, MultiBiorbdModel
 from .interfaces.biomodel import BioModel
-from .limits.constraints import ConstraintFcn, ConstraintList, Constraint
+from .limits.constraints import ConstraintFcn, ConstraintList, Constraint, ParameterConstraintList
 from .limits.phase_transition import PhaseTransitionFcn, PhaseTransitionList, PhaseTransition
 from .limits.multinode_constraint import MultinodeConstraintFcn, MultinodeConstraintList, MultinodeConstraint
 from .limits.multinode_objective import MultinodeObjectiveFcn, MultinodeObjectiveList, MultinodeObjective
-from .limits.objective_functions import ObjectiveFcn, ObjectiveList, Objective
-from .limits.path_conditions import (
-    BoundsList,
-    Bounds,
-    InitialGuessList,
-    InitialGuess,
-    NoisedInitialGuess,
-)
+from .limits.objective_functions import ObjectiveFcn, ObjectiveList, Objective, ParameterObjectiveList
+from .limits.path_conditions import BoundsList, InitialGuessList
 from .limits.fatigue_path_conditions import FatigueBounds, FatigueInitialGuess
 from .limits.penalty_controller import PenaltyController
 from .misc.enums import (
@@ -217,6 +211,7 @@
 )
 from .optimization.parameters import ParameterList
 from .optimization.solution import Solution
-from .optimization.optimization_variable import OptimizationVariableList, VariableScalingList, VariableScaling
+from .optimization.optimization_variable import OptimizationVariableList
+from .optimization.variable_scaling import VariableScalingList, VariableScaling
 
 from .misc.casadi_expand import lt, le, gt, ge, if_else, if_else_zero

bioptim/dynamics/configure_problem.py

@@ -21,7 +21,6 @@
 from ..misc.mapping import BiMapping, Mapping
 from ..misc.options import UniquePerPhaseOptionList, OptionGeneric
 from ..limits.constraints import ImplicitConstraintFcn
-from ..optimization.optimization_variable import VariableScaling
 
 
 class ConfigureProblem:
@@ -202,9 +201,9 @@ def torque_driven(
                     )
 
         # Declared rigidbody states and controls
-        ConfigureProblem.configure_q(ocp, nlp, True, False)
-        ConfigureProblem.configure_qdot(ocp, nlp, True, False, True)
-        ConfigureProblem.configure_tau(ocp, nlp, False, True, fatigue)
+        ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False)
+        ConfigureProblem.configure_qdot(ocp, nlp, as_states=True, as_controls=False, as_states_dot=True)
+        ConfigureProblem.configure_tau(ocp, nlp, as_states=False, as_controls=True, fatigue=fatigue)
 
         if (
             rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS
@@ -603,7 +602,7 @@ def configure_dynamics_function(ocp, nlp, dyn_func, expand: bool = True, **extra
             If the dynamics should be expanded with casadi
         """
 
-        nlp.parameters = ocp.v.parameters_in_list
+        nlp.parameters = ocp.parameters
         DynamicsFunctions.apply_parameters(nlp.parameters.mx, nlp)
 
         dynamics_eval = dyn_func(
@@ -808,7 +807,7 @@ def _manage_fatigue_to_new_variable(
             lambda t, x, u, p: x[:n_elements, :] * np.nan,
             plot_type=PlotType.INTEGRATED,
             legend=legend,
-            bounds=Bounds(-1, 1),
+            bounds=Bounds(None, -1, 1),
         )
         control_plot_name = f"{name}_controls" if not multi_interface and split_controls else f"{name}"
         nlp.plot[control_plot_name] = CustomPlot(
@@ -953,7 +952,7 @@ def define_cx_unscaled(_cx_scaled: list, scaling: np.ndarray) -> list:
             name, name_elements, ocp, nlp, as_states, as_controls, fatigue
         ):
             # If the element is fatigable, this function calls back configure_new_variable to fill everything.
-            # Therefore, we can exist now
+            # Therefore, we can exit now
             return
 
         if name not in nlp.variable_mappings:
@@ -984,18 +983,19 @@ def define_cx_unscaled(_cx_scaled: list, scaling: np.ndarray) -> list:
             and name in ocp.nlp[nlp.use_states_dot_from_phase_idx].states_dot[0]
         )
 
+        # Declare the x_init for that variable
+        if as_states and name not in nlp.x_init:
+            nlp.x_init.add(name, initial_guess=np.zeros(len(nlp.variable_mappings[name].to_first.map_idx)))
+        if as_controls and name not in nlp.u_init:
+            nlp.u_init.add(name, initial_guess=np.zeros(len(nlp.variable_mappings[name].to_first.map_idx)))
+
+        # Declare the scaling for that variable
         if as_states and name not in nlp.x_scaling:
-            nlp.x_scaling[name] = VariableScaling(
-                key=name, scaling=np.ones(len(nlp.variable_mappings[name].to_first.map_idx))
-            )
+            nlp.x_scaling.add(name, scaling=np.ones(len(nlp.variable_mappings[name].to_first.map_idx)))
         if as_states_dot and name not in nlp.xdot_scaling:
-            nlp.xdot_scaling[name] = VariableScaling(
-                key=name, scaling=np.ones(len(nlp.variable_mappings[name].to_first.map_idx))
-            )
+            nlp.xdot_scaling.add(name, scaling=np.ones(len(nlp.variable_mappings[name].to_first.map_idx)))
         if as_controls and name not in nlp.u_scaling:
-            nlp.u_scaling[name] = VariableScaling(
-                key=name, scaling=np.ones(len(nlp.variable_mappings[name].to_first.map_idx))
-            )
+            nlp.u_scaling.add(name, scaling=np.ones(len(nlp.variable_mappings[name].to_first.map_idx)))
 
         # Use of states[0] and controls[0] is permitted since ocp.assume_phase_dynamics is True
         mx_states = [] if not copy_states else [ocp.nlp[nlp.use_states_from_phase_idx].states[0][name].mx]

bioptim/dynamics/dynamics_functions.py

@@ -720,12 +720,10 @@ def apply_parameters(parameters: MX.sym, nlp):
             The definition of the system
         """
 
-        offset = 0
         for param in nlp.parameters:
             # Call the pre dynamics function
             if param.function[0]:
-                param.function[0](nlp.model, parameters[offset : offset + param.size], **param.params)
-                offset += param.size
+                param.function[0](nlp.model, parameters[param.index], **param.params)
 
     @staticmethod
     def compute_qdot(nlp: NonLinearProgram, q: MX | SX, qdot: MX | SX):

bioptim/dynamics/integrator.py

@@ -71,7 +71,7 @@ def __init__(self, ode: dict, ode_opt: dict):
         self.cx = ode_opt["cx"]
         self.x_sym = ode["x_scaled"]
         self.u_sym = [] if ode_opt["control_type"] is ControlType.NONE else ode["p_scaled"]
-        self.param_sym = ode_opt["param"].cx_start
+        self.param_sym = ode_opt["param"].cx
         self.param_scaling = ode_opt["param"].scaling
         self.fun = ode["ode"]
         self.implicit_fun = ode["implicit_ode"]
@@ -123,7 +123,7 @@ def get_u(self, u: np.ndarray, dt_norm: float) -> np.ndarray:
         else:
             raise RuntimeError(f"{self.control_type} ControlType not implemented yet")
 
-    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX) -> tuple:
+    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX, param_scaling) -> tuple:
         """
         The dynamics of the system
 
@@ -153,7 +153,7 @@ def _finish_init(self):
         self.function = Function(
             "integrator",
             [self.x_sym, self.u_sym, self.param_sym],
-            self.dxdt(self.h, self.x_sym, self.u_sym, self.param_sym * self.param_scaling),
+            self.dxdt(self.h, self.x_sym, self.u_sym, self.param_sym, self.param_scaling),
             ["x0", "p", "params"],
             ["xf", "xall"],
         )
@@ -219,7 +219,7 @@ def next_x(self, h: float, t: float, x_prev: MX | SX, u: MX | SX, p: MX | SX):
 
         raise RuntimeError("RK is abstract, please select a specific RK")
 
-    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX) -> tuple:
+    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX, param_scaling) -> tuple:
         """
         The dynamics of the system
 
@@ -241,7 +241,7 @@ def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX) ->
 
         u = controls
         x = self.cx(states.shape[0], self.n_step + 1)
-        p = params
+        p = params * param_scaling
         x[:, 0] = states
 
         for i in range(1, self.n_step + 1):
@@ -574,7 +574,7 @@ def get_u(self, u: np.ndarray, dt_norm: float) -> np.ndarray:
         else:
             raise NotImplementedError(f"{self.control_type} ControlType not implemented yet with COLLOCATION")
 
-    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX) -> tuple:
+    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX, param_scaling) -> tuple:
         """
         The dynamics of the system
 
@@ -604,10 +604,14 @@ def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX) ->
                 xp_j += self._c[r, j] * states[r]
 
             if self.defects_type == DefectType.EXPLICIT:
-                f_j = self.fun(states[j], self.get_u(controls, self.step_time[j]), params)[:, self.idx]
+                f_j = self.fun(states[j], self.get_u(controls, self.step_time[j]), params * param_scaling)[:, self.idx]
                 defects.append(h * f_j - xp_j)
             elif self.defects_type == DefectType.IMPLICIT:
-                defects.append(self.implicit_fun(states[j], self.get_u(controls, self.step_time[j]), params, xp_j / h))
+                defects.append(
+                    self.implicit_fun(
+                        states[j], self.get_u(controls, self.step_time[j]), params * param_scaling, xp_j / h
+                    )
+                )
             else:
                 raise ValueError("Unknown defects type. Please use 'explicit' or 'implicit'")
 
@@ -626,7 +630,7 @@ def _finish_init(self):
         self.function = Function(
             "integrator",
             [horzcat(*self.x_sym), self.u_sym, self.param_sym],
-            self.dxdt(self.h, self.x_sym, self.u_sym, self.param_sym * self.param_scaling),
+            self.dxdt(self.h, self.x_sym, self.u_sym, self.param_sym, self.param_scaling),
             ["x0", "p", "params"],
             ["xf", "xall", "defects"],
         )
@@ -656,7 +660,7 @@ def __init__(self, ode: dict, ode_opt: dict):
 
         super(IRK, self).__init__(ode, ode_opt)
 
-    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX) -> tuple:
+    def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX, param_scaling) -> tuple:
         """
         The dynamics of the system
 
@@ -677,7 +681,7 @@ def dxdt(self, h: float, states: MX | SX, controls: MX | SX, params: MX | SX) ->
         """
 
         nx = states[0].shape[0]
-        _, _, defect = super(IRK, self).dxdt(h, states, controls, params)
+        _, _, defect = super(IRK, self).dxdt(h, states, controls, params, param_scaling)
 
         # Root-finding function, implicitly defines x_collocation_points as a function of x0 and p
         vfcn = Function("vfcn", [vertcat(*states[1:]), states[0], controls, params], [defect]).expand()
@@ -702,7 +706,7 @@ def _finish_init(self):
         self.function = Function(
             "integrator",
             [self.x_sym[0], self.u_sym, self.param_sym],
-            self.dxdt(self.h, self.x_sym, self.u_sym, self.param_sym * self.param_scaling),
+            self.dxdt(self.h, self.x_sym, self.u_sym, self.param_sym, self.param_scaling),
             ["x0", "p", "params"],
             ["xf", "xall"],
         )

bioptim/dynamics/ode_solver.py

@@ -422,7 +422,7 @@ def integrator(self, ocp, nlp, node_index: int) -> list:
 
             if not isinstance(ocp.cx(), MX):
                 raise RuntimeError("use_sx=True and OdeSolver.CVODES are not yet compatible")
-            if ocp.v.parameters_in_list.shape != 0:
+            if ocp.parameters.shape != 0:
                 raise RuntimeError("CVODES cannot be used while optimizing parameters")
             if nlp.external_forces:
                 raise RuntimeError("CVODES cannot be used with external_forces")
@@ -435,7 +435,7 @@ def integrator(self, ocp, nlp, node_index: int) -> list:
                 "ode": nlp.dynamics_func(
                     nlp.states.scaled.cx_start,
                     nlp.controls.scaled.cx_start,
-                    nlp.parameters.cx_start,
+                    nlp.parameters.cx,
                 ),
             }
             ode_opt = {"t0": 0, "tf": nlp.dt}
@@ -448,7 +448,7 @@ def integrator(self, ocp, nlp, node_index: int) -> list:
                     [
                         nlp.states.scaled.cx_start,
                         nlp.controls.scaled.cx_start,
-                        nlp.parameters.cx_start,
+                        nlp.parameters.cx,
                     ],
                     self._adapt_integrator_output(
                         integrator_func,

bioptim/examples/acados/cube.py

@@ -13,8 +13,7 @@
     DynamicsFcn,
     ObjectiveFcn,
     ObjectiveList,
-    Bounds,
-    InitialGuess,
+    BoundsList,
     OdeSolver,
     Solver,
 )
@@ -28,23 +27,22 @@ def prepare_ocp(biorbd_model_path, n_shooting, tf, ode_solver=OdeSolver.RK4(), u
     dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
 
     # Path constraint
-    x_bounds = bio_model.bounds_from_ranges(["q", "qdot"])
-    x_init = InitialGuess([0] * (bio_model.nb_q + bio_model.nb_qdot))
+    x_bounds = BoundsList()
+    x_bounds["q"] = bio_model.bounds_from_ranges("q")
+    x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot")
 
     # Define control path constraint
-    tau_min, tau_max, tau_init = -100, 100, 0
-    u_bounds = Bounds([tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau)
-    u_init = InitialGuess([tau_init] * bio_model.nb_tau)
+    tau_min, tau_max = -100, 100
+    u_bounds = BoundsList()
+    u_bounds["tau"] = [tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau
 
     return OptimalControlProgram(
         bio_model,
         dynamics,
         n_shooting,
         tf,
-        x_init,
-        u_init,
-        x_bounds,
-        u_bounds,
+        x_bounds=x_bounds,
+        u_bounds=u_bounds,
         ode_solver=ode_solver,
         use_sx=use_sx,
         assume_phase_dynamics=True,

bioptim/examples/acados/pendulum.py

@@ -16,7 +16,6 @@
     DynamicsList,
     DynamicsFcn,
     BoundsList,
-    InitialGuessList,
     Solver,
 )
 
@@ -68,22 +67,17 @@ def prepare_ocp(
 
     # Path constraint
     x_bounds = BoundsList()
-    x_bounds.add(bounds=bio_model.bounds_from_ranges(["q", "qdot"]))
-    x_bounds[0][:, 0] = 0
-
-    # Initial guess
-    x_init = InitialGuessList()
-    x_init.add([0] * (nq + nqdot))
+    x_bounds["q"] = bio_model.bounds_from_ranges("q")
+    x_bounds["q"][:, 0] = 0
+    x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot")
+    x_bounds["qdot"][:, 0] = 0
 
     # Define control path constraint
     n_tau = bio_model.nb_tau
-    torque_min, torque_max, torque_init = -300, 300, 0
+    torque_min, torque_max = -300, 300
     u_bounds = BoundsList()
-    u_bounds.add([torque_min] * n_tau, [torque_max] * n_tau)
-    u_bounds[0][n_tau - 1, :] = 0
-
-    u_init = InitialGuessList()
-    u_init.add([torque_init] * n_tau)
+    u_bounds["tau"] = [torque_min] * n_tau, [torque_max] * n_tau
+    u_bounds["tau"][-1, :] = 0
 
     # ------------- #
 
@@ -92,11 +86,9 @@ def prepare_ocp(
         dynamics,
         n_shooting,
         final_time,
-        x_init,
-        u_init,
-        x_bounds,
-        u_bounds,
-        objective_functions,
+        x_bounds=x_bounds,
+        u_bounds=u_bounds,
+        objective_functions=objective_functions,
         use_sx=use_sx,
         assume_phase_dynamics=True,
     )