Merge pull request #843 from pariterre/master

Implemented time in dynamic

bioptim/dynamics/configure_problem.py

@@ -1,6 +1,6 @@
 from typing import Callable, Any
 
-from casadi import vertcat, Function, DM, MX
+from casadi import vertcat, Function, DM
 
 from .configure_new_variable import NewVariableConfiguration
 from .dynamics_functions import DynamicsFunctions
@@ -558,7 +558,7 @@ def stochastic_torque_driven_free_floating_base(
         ConfigureProblem.torque_driven_free_floating_base(
             ocp=ocp,
             nlp=nlp,
-            with_contact=with_contact,
+            with_contact=with_contact,  # TODO : this should be removed
             with_friction=with_friction,
         )
 

bioptim/dynamics/integrator.py

@@ -252,7 +252,7 @@ def _time_xall_from_dt_func(self) -> Function:
 
     @property
     def h(self):
-        return (self.t_span_sym[1] - self.t_span_sym[0]) / self._n_step
+        return self.t_span_sym[1] / self._n_step
 
     def next_x(self, t0: float | MX | SX, x_prev: MX | SX, u: MX | SX, p: MX | SX, a: MX | SX) -> MX | SX:
         """
@@ -431,7 +431,7 @@ def shape_xall(self):
 
     @property
     def h(self):
-        return self.t_span_sym[1] - self.t_span_sym[0]
+        return self.t_span_sym[1]
 
     def dxdt(
         self,
@@ -548,7 +548,7 @@ def _output_names(self):
 
     @property
     def h(self):
-        return self.t_span_sym[1] - self.t_span_sym[0]
+        return self.t_span_sym[1]
 
     @property
     def _integration_time(self):
@@ -564,9 +564,7 @@ def shape_xall(self):
 
     @property
     def _time_xall_from_dt_func(self) -> Function:
-        return Function(
-            "step_time", [self.t_span_sym], [self.t_span_sym[0] + (self._integration_time + [1]) * self.t_span_sym[1]]
-        )
+        return Function("step_time", [self.t_span_sym], [self.t_span_sym[0] + (self._integration_time + [1]) * self.h])
 
     def get_u(self, u: np.ndarray, t: float | MX | SX) -> np.ndarray:
         """
@@ -634,7 +632,8 @@ def dxdt(
 
         # Concatenate constraints
         defects = vertcat(*defects)
-        return states_end, horzcat(*states), defects
+        collocation_states = horzcat(*states) if self.duplicate_starting_point else horzcat(*states[1:])
+        return states_end, collocation_states, defects
 
 
 class IRK(COLLOCATION):

bioptim/examples/getting_started/example_external_forces.py

@@ -9,6 +9,7 @@
 It is expected to act on a segment in the global_reference_frame. BiorbdBioptim expect a list of list[segment_name, vector]
 where the vector is a 6x1 array (Mx, My, Mz, Fx, Fy, Fz)
 """
+
 import platform
 
 from bioptim import (
@@ -75,8 +76,8 @@ def prepare_ocp(
     dynamics = DynamicsList()
     dynamics.add(
         # This must be PhaseDynamics.ONE_PER_NODE since external forces change at each node within the phase
-        DynamicsFcn.TORQUE_DRIVEN, 
-        expand_dynamics=expand_dynamics, 
+        DynamicsFcn.TORQUE_DRIVEN,
+        expand_dynamics=expand_dynamics,
         phase_dynamics=PhaseDynamics.ONE_PER_NODE,
         external_forces=external_forces,
     )

bioptim/examples/getting_started/example_multinode_objective.py

@@ -22,12 +22,12 @@
 )
 
 
-def multinode_min_controls(controllers: list[PenaltyController]) -> MX:
+def multinode_min_controls(controllers: list[PenaltyController]):
     """
     This function mimics the ObjectiveFcn.MINIMIZE_CONTROLS with a multinode objective.
     Note that it is better to use ObjectiveFcn.MINIMIZE_CONTROLS, here is juste a toy example.
     """
-    dt = controllers[0].tf / controllers[0].ns
+    dt = controllers[0].dt.cx
     out = 0
     for i, ctrl in enumerate(controllers):
         out += sum1(ctrl.controls["tau"].cx_start ** 2) * dt

bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py

@@ -284,7 +284,7 @@ def expected_feedback_effort(controllers: list[PenaltyController], sensory_noise
     sensory_noise_magnitude : cas.DM
         Magnitude of the sensory noise.
     """
-    dt = controllers[0].tf / controllers[0].ns
+    dt = controllers[0].dt.cx
     sensory_noise_matrix = sensory_noise_magnitude * cas.MX_eye(4)
 
     # create the casadi function to be evaluated

bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py

@@ -127,7 +127,7 @@ def minimize_uncertainty(controllers: list[PenaltyController], key: str) -> cas.
     """
     Minimize the uncertainty (covariance matrix) of the states "key".
     """
-    dt = controllers[0].tf / controllers[0].ns
+    dt = controllers[0].dt.cx
     out: Any = 0
     for i, ctrl in enumerate(controllers):
         cov_matrix = StochasticBioModel.reshape_to_matrix(ctrl.integrated_values["cov"].cx, ctrl.model.matrix_shape_cov)
@@ -270,7 +270,7 @@ def expected_feedback_effort(controllers: list[PenaltyController]) -> cas.MX:
         List of controllers to be used to compute the expected effort.
     """
 
-    dt = controllers[0].tf / controllers[0].ns
+    dt = controllers[0].dt.cx
     sensory_noise_matrix = controllers[0].model.sensory_noise_magnitude * cas.MX_eye(4)
 
     # create the casadi function to be evaluated

bioptim/gui/check_conditioning.py

@@ -67,7 +67,6 @@ def jacobian_hessian_constraints():
                 nlp.states_dot.node_index = node_index
                 nlp.controls.node_index = node_index
                 nlp.algebraic_states.node_index = node_index
-                time = ocp.node_time(phase_idx=nlp.phase_idx, node_idx=node_index)
 
                 if constraints.multinode_penalty:
                     n_phases = ocp.n_phases
@@ -85,8 +84,8 @@ def jacobian_hessian_constraints():
 
                     for controller in controllers:
                         controller.node_index = constraints.node_idx[0]
-                    t0 = PenaltyHelpers.t0(constraints, controllers[0].ocp)
-                    _, x, u, a = constraints.get_variable_inputs(controllers)
+
+                    _, t0, x, u, a = constraints.get_variable_inputs(controllers)
                     p = nlp.parameters.cx
 
                     list_constraints.append(
@@ -188,8 +187,7 @@ def jacobian_hessian_constraints():
 
                         for controller in controllers:
                             controller.node_index = constraints.node_idx[0]
-                        t0 = PenaltyHelpers.t0(constraints, controllers[0].ocp)
-                        _, x, u, a = constraints.get_variable_inputs(controllers)
+                        _, t0, x, u, a = constraints.get_variable_inputs(controllers)
                         p = nlp.parameters.cx
 
                         hessian_cas = hessian(
@@ -392,8 +390,7 @@ def hessian_objective():
 
                 for controller in controllers:
                     controller.node_index = obj.node_idx[0]
-                t0 = PenaltyHelpers.t0(obj, controllers[0].ocp)
-                _, x, u, s = obj.get_variable_inputs(controllers)
+                _, t0, x, u, s = obj.get_variable_inputs(controllers)
                 params = nlp.parameters.cx
                 target = PenaltyHelpers.target(obj, obj.node_idx.index(node_index))
 

bioptim/gui/plot.py

@@ -756,7 +756,7 @@ def _compute_y_from_plot_func(
             node_idx = custom_plot.node_idx[idx]
             if "penalty" in custom_plot.parameters:
                 penalty = custom_plot.parameters["penalty"]
-                t0 = PenaltyHelpers.t0(penalty, self.ocp)
+                t0 = PenaltyHelpers.t0(penalty, idx, lambda p_idx, n_idx: time_stepwise[p_idx][n_idx][0])
 
                 x_node = PenaltyHelpers.states(
                     penalty,

bioptim/interfaces/interface_utils.py

@@ -294,7 +294,7 @@ def generic_get_all_penalties(interface, nlp: NonLinearProgram, penalties, scale
 
 
 def _get_weighted_function_inputs(penalty, penalty_idx, ocp, nlp, scaled):
-    t0 = PenaltyHelpers.t0(penalty, ocp)
+    t0 = PenaltyHelpers.t0(penalty, penalty_idx, lambda p, n: ocp.node_time(phase_idx=p, node_idx=n))
 
     weight = PenaltyHelpers.weight(penalty)
     target = PenaltyHelpers.target(penalty, penalty_idx)

bioptim/interfaces/solve_ivp_interface.py

@@ -1,7 +1,10 @@
 from typing import Callable, Any
+
+from casadi import vertcat
 import numpy as np
 from scipy.integrate import solve_ivp
 from scipy.interpolate import interp1d
+
 from ..misc.enums import Shooting, ControlType, SolutionIntegrator
 
 
@@ -45,11 +48,12 @@ def solve_ivp_interface(
 
     y = []
     control_type = nlp.control_type
+
     for node in range(nlp.ns):
-        # TODO WARNING NEXT LINE IS A BUG DELIBERATELY INTRODUCED TO HAVE THE TESTS PASS. TIME SHOULD BE HANDLED
-        # PROPERLY AS THE COMMENTED LINE SUGGEST
-        t_span = t[0]
-        # t_span = t[node]
+        if method == SolutionIntegrator.OCP:
+            t_span = vertcat(t[node][0], t[node][1] - t[node][0])
+        else:
+            t_span = t[node]
         t_eval = np.linspace(float(t_span[0]), float(t_span[1]), nlp.n_states_stepwise_steps(node))
 
         # If multiple shooting, we need to set the first x0, otherwise use the previous answer

bioptim/limits/constraints.py

@@ -342,7 +342,7 @@ def time_constraint(_: Constraint, controller: PenaltyController, **unused_param
                 Since the function does nothing, we can safely ignore any argument
             """
 
-            return controller.tf
+            return controller.tf.cx
 
         @staticmethod
         def bound_state(
@@ -651,7 +651,7 @@ def stochastic_covariance_matrix_continuity_implicit(
             ) * CX_eye(
                 vertcat(controller.model.sensory_noise_magnitude, controller.model.motor_noise_magnitude).shape[0]
             )
-            dt = controller.tf / controller.ns
+            dt = controller.dt.cx
             dg_dw = -dt * c_matrix
             dg_dx = -CX_eye(a_matrix.shape[0]) - dt / 2 * a_matrix
 
@@ -677,7 +677,7 @@ def stochastic_df_dx_implicit(
             if not controller.get_nlp.is_stochastic:
                 raise RuntimeError("This function is only valid for stochastic problems")
 
-            dt = controller.tf / controller.ns
+            dt = controller.dt.cx
 
             nb_root = controller.model.nb_root
             # TODO: Charbie -> This is only True for x=[q, qdot], u=[tau] (have to think on how to generalize it)
@@ -695,7 +695,7 @@ def stochastic_df_dx_implicit(
             algebraic_states_sym = MX.sym("algebraic_states_sym", controller.algebraic_states.shape, 1)
 
             dx = controller.extra_dynamics(0)(
-                vertcat(controller.time.mx, controller.time.mx + controller.dt.mx),
+                controller.t_span.mx,
                 vertcat(q_root, q_joints, qdot_root, qdot_joints),  # States
                 tau_joints,
                 controller.parameters.mx,
@@ -708,7 +708,7 @@ def stochastic_df_dx_implicit(
             DF_DX_fun = Function(
                 "DF_DX_fun",
                 [
-                    vertcat(controller.time.mx, controller.dt.mx),
+                    controller.t_span.mx,
                     q_root,
                     q_joints,
                     qdot_root,
@@ -725,7 +725,7 @@ def stochastic_df_dx_implicit(
             parameters[controller.parameters["sensory_noise"].index] = controller.model.sensory_noise_magnitude
 
             DF_DX = DF_DX_fun(
-                vertcat(controller.time.cx, controller.dt.cx),
+                controller.t_span.cx,
                 controller.q.cx[:nb_root],
                 controller.q.cx[nb_root:],
                 controller.qdot.cx[:nb_root],
@@ -766,7 +766,7 @@ def stochastic_helper_matrix_collocation(
             parameters[controller.parameters["sensory_noise"].index] = controller.model.sensory_noise_magnitude
 
             constraint = Mc(
-                vertcat(controller.time.cx, controller.dt.cx),
+                controller.t_span.cx,
                 controller.states.cx_start,
                 horzcat(*(controller.states.cx_intermediates_list)),
                 controller.controls.cx_start,
@@ -804,7 +804,7 @@ def stochastic_covariance_matrix_continuity_collocation(
             parameters[controller.parameters["sensory_noise"].index] = controller.model.sensory_noise_magnitude
 
             cov_next_computed = Pf(
-                vertcat(controller.time.cx, controller.dt.cx),
+                controller.t_span.cx,
                 controller.states.cx_start,
                 horzcat(*(controller.states.cx_intermediates_list)),
                 controller.controls.cx_start,
@@ -844,17 +844,15 @@ def stochastic_mean_sensory_input_equals_reference(
             sensory_input = Function(
                 "tp",
                 [
-                    controller.time.mx,
-                    controller.dt.mx,
+                    controller.t_span.mx,
                     controller.states.mx,
                     controller.controls.mx,
                     controller.parameters.mx,
                     controller.algebraic_states.mx,
                 ],
                 [sensory_input],
             )(
-                controller.time.cx,
-                controller.dt.cx,
+                controller.t_span.cx,
                 controller.states.cx_start,
                 controller.controls.cx_start,
                 controller.parameters.cx,
@@ -923,7 +921,7 @@ def collocation_jacobians(penalty, controller):
             )
 
             xf, _, defects = controller.integrate_extra_dynamics(0).function(
-                vertcat(controller.time.cx, controller.time.cx + controller.dt.cx),
+                vertcat(controller.t_span.cx),
                 horzcat(controller.states_scaled.cx, horzcat(*controller.states_scaled.cx_intermediates_list)),
                 controller.controls_scaled.cx,
                 controller.parameters.cx,  # TODO: fix parameter scaling
@@ -942,7 +940,7 @@ def collocation_jacobians(penalty, controller):
             Mc = Function(
                 "M_cons",
                 [
-                    vertcat(controller.time.cx, controller.dt.cx),
+                    controller.t_span.cx,
                     controller.states_scaled.cx_start,
                     horzcat(*controller.states_scaled.cx_intermediates_list),
                     controller.controls_scaled.cx_start,
@@ -958,7 +956,7 @@ def collocation_jacobians(penalty, controller):
             Pf = Function(
                 "P_next",
                 [
-                    vertcat(controller.time.cx, controller.dt.cx),
+                    controller.t_span.cx,
                     controller.states_scaled.cx_start,
                     horzcat(*controller.states_scaled.cx_intermediates_list),
                     controller.controls_scaled.cx_start,
@@ -973,7 +971,7 @@ def collocation_jacobians(penalty, controller):
             Gdx_fun = Function(
                 "Gdx_fun",
                 [
-                    vertcat(controller.time.cx, controller.dt.cx),
+                    controller.t_span.cx,
                     controller.states_scaled.cx_start,
                     horzcat(*controller.states_scaled.cx_intermediates_list),
                     controller.controls_scaled.cx_start,
@@ -986,7 +984,7 @@ def collocation_jacobians(penalty, controller):
             Gdz_fun = Function(
                 "Gdz_fun",
                 [
-                    vertcat(controller.time.cx, controller.dt.cx),
+                    controller.t_span.cx,
                     controller.states_scaled.cx_start,
                     horzcat(*controller.states_scaled.cx_intermediates_list),
                     controller.controls_scaled.cx_start,
@@ -999,7 +997,7 @@ def collocation_jacobians(penalty, controller):
             Gdw_fun = Function(
                 "Gdw_fun",
                 [
-                    vertcat(controller.time.cx, controller.dt.cx),
+                    controller.t_span.cx,
                     controller.states_scaled.cx_start,
                     horzcat(*controller.states_scaled.cx_intermediates_list),
                     controller.controls_scaled.cx_start,
@@ -1012,7 +1010,7 @@ def collocation_jacobians(penalty, controller):
             Fdz_fun = Function(
                 "Fdz_fun",
                 [
-                    vertcat(controller.time.cx, controller.dt.cx),
+                    controller.t_span.cx,
                     controller.states_scaled.cx_start,
                     horzcat(*controller.states_scaled.cx_intermediates_list),
                     controller.controls_scaled.cx_start,

bioptim/limits/multinode_penalty.py

@@ -321,7 +321,7 @@ def time_equality(penalty, controllers: list[PenaltyController, PenaltyControlle
 
             MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers)
 
-            times = [controller.tf for i, controller in enumerate(controllers)]
+            times = [controller.tf.cx for i, controller in enumerate(controllers)]
 
             time_0 = times[0]
             out = controllers[0].cx.zeros((1, 1))
@@ -348,7 +348,7 @@ def track_total_time(penalty, controllers: list[PenaltyController, PenaltyContro
             """
 
             MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers)
-            return sum([controller.tf for i, controller in enumerate(controllers)])
+            return sum([controller.tf.cx for i, controller in enumerate(controllers)])
 
         @staticmethod
         def stochastic_helper_matrix_explicit(

bioptim/limits/objective_functions.py

@@ -250,7 +250,7 @@ def minimize_time(
                     taken into account elsewhere in the code)
                 """
 
-                return controller.tf
+                return controller.time.cx
 
         @staticmethod
         def get_dt(_):