[WIP] defects, contacts, implicit constraints, multinode constraints with cx_intermediates, ...

README.md

@@ -177,7 +177,6 @@ As a tour guide that uses this binder, you can watch the `bioptim` workshop that
     - [CostType](#enum-costtype)
     - [SolutionIntegrator](#enum-solutionintegrator)
     - [QuadratureRule](#enum-quadraturerule)
-    - [SoftContactDynamics](#enum-softcontactdynamics)
     - [DefectType](#enum-defecttype)
 
     </details>
@@ -707,7 +706,6 @@ OptimalControlProgram(
     objective_functions: [Objective, ObjectiveList],
     constraints: [Constraint, ConstraintList],
     parameters: ParameterList,
-    ode_solver: OdeSolver,
     control_type: [ControlType, list],
     all_generalized_mapping: BiMapping,
     q_mapping: BiMapping,
@@ -1760,18 +1758,6 @@ The type of integration used to integrate the cost function terms of Lagrange:
 - APPROXIMATE_TRAPEZOIDAL: The integral is approximated by a trapezoidal rule using the state at the beginning of the next interval.
 - TRAPEZOIDAL: The integral is approximated by a trapezoidal rule using the state at the end of the current interval.
 
-### Enum: SoftContactDynamics
-The type of transcription of any dynamics (e.g., rigidbody_dynamics or soft_contact_dynamics):
-- ODE: soft contact dynamics is handled explicitly.
-- CONSTRAINT: an extra control *fext* is added, and it ensures respecting soft contact_dynamics on nodes through a constraint.
-
-### Enum: DefectType
-- EXPLICIT: The defect comes from the explicit formulation.
-- IMPLICIT: The defect comes from the implicit formulation.
-- NOT_APPLICABLE: The defect is not applicable.
-
-
-
 # Examples
 In this section, we describe all the examples implemented with bioptim. They are ordered in separate files. Each subsection corresponds to the different files, dealing with different examples and topics.
 Please note that the examples from the paper (see [Citing](#citing)) can be found in this repo

bioptim/__init__.py

@@ -213,16 +213,16 @@
     VariableType,
     SolutionIntegrator,
     QuadratureRule,
-    SoftContactDynamics,
     DefectType,
     MagnitudeType,
     MultiCyclicCycleSolutions,
     PhaseDynamics,
     OnlineOptim,
+    ContactType,
 )
 from .misc.mapping import BiMappingList, BiMapping, Mapping, SelectionMapping, Dependency
 from .models.biorbd.biorbd_model import BiorbdModel
-from .models.biorbd.external_forces import ExternalForceSetTimeSeries
+from .models.biorbd.external_forces import ExternalForceSetTimeSeries, ExternalForceSetVariables
 from .models.biorbd.holonomic_biorbd_model import HolonomicBiorbdModel
 from .models.biorbd.multi_biorbd_model import MultiBiorbdModel
 from .models.biorbd.stochastic_biorbd_model import StochasticBiorbdModel

bioptim/dynamics/configure_new_variable.py

@@ -17,7 +17,6 @@ def __init__(
         nlp,
         as_states: bool,
         as_controls: bool,
-        as_states_dot: bool = False,
         as_algebraic_states: bool = False,
         fatigue: FatigueList = None,
         combine_name: str = None,
@@ -40,8 +39,6 @@ def __init__(
             A reference to the phase
         as_states: bool
             If the new variable should be added to the state variable set
-        as_states_dot: bool
-            If the new variable should be added to the state_dot variable set
         as_controls: bool
             If the new variable should be added to the control variable set
         as_algebraic_states: bool
@@ -64,7 +61,6 @@ def __init__(
         self.nlp = nlp
         self.as_states = as_states
         self.as_controls = as_controls
-        self.as_states_dot = as_states_dot
         self.as_algebraic_states = as_algebraic_states
         self.fatigue = fatigue
         self.combine_name = combine_name
@@ -184,10 +180,6 @@ def _declare_variable_scaling(self):
             self.nlp.x_scaling.add(
                 self.name, scaling=np.ones(len(self.nlp.variable_mappings[self.name].to_first.map_idx))
             )
-        if self.as_states_dot and self.name not in self.nlp.xdot_scaling:
-            self.nlp.xdot_scaling.add(
-                self.name, scaling=np.ones(len(self.nlp.variable_mappings[self.name].to_first.map_idx))
-            )
         if self.as_controls and self.name not in self.nlp.u_scaling:
             self.nlp.u_scaling.add(
                 self.name, scaling=np.ones(len(self.nlp.variable_mappings[self.name].to_first.map_idx))
@@ -217,6 +209,7 @@ def _declare_legend(self):
 
     def _declare_cx_and_plot(self):
         if self.as_states:
+            # States
             for node_index in range(
                 self.nlp.n_states_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1
             ):
@@ -243,6 +236,21 @@ def _declare_cx_and_plot(self):
                         combine_to=self.combine_name,
                     )
 
+            # States dot
+            for node_index in range(
+                self.nlp.n_states_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1
+            ):
+                n_cx = self.nlp.ode_solver.n_required_cx + 2
+                cx_scaled = self.define_cx_scaled(n_col=n_cx, node_index=node_index)
+                cx = self.define_cx_unscaled(cx_scaled, np.ones_like(self.nlp.x_scaling[self.name].scaling))
+                self.nlp.states_dot.append(
+                    self.name,
+                    cx,
+                    cx_scaled,
+                    self.nlp.variable_mappings[self.name],
+                    node_index,
+                )
+
         if self.as_controls:
             for node_index in range(
                 self.nlp.n_controls_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1
@@ -276,21 +284,6 @@ def _declare_cx_and_plot(self):
                         ),
                     )
 
-        if self.as_states_dot:
-            for node_index in range(
-                self.nlp.n_states_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1
-            ):
-                n_cx = self.nlp.ode_solver.n_required_cx + 2
-                cx_scaled = self.define_cx_scaled(n_col=n_cx, node_index=node_index)
-                cx = self.define_cx_unscaled(cx_scaled, self.nlp.xdot_scaling[self.name].scaling)
-                self.nlp.states_dot.append(
-                    self.name,
-                    cx,
-                    cx_scaled,
-                    self.nlp.variable_mappings[self.name],
-                    node_index,
-                )
-
         if self.as_algebraic_states:
             for node_index in range(
                 self.nlp.n_states_nodes if self.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE else 1
@@ -403,7 +396,13 @@ def _manage_fatigue_to_new_variable(
 
         if split_controls:
             NewVariableConfiguration(
-                var_names_with_suffix[-1], name_elements, ocp, nlp, as_states, as_controls, skip_plot=True
+                var_names_with_suffix[-1],
+                name_elements,
+                ocp,
+                nlp,
+                as_states=as_states,
+                as_controls=as_controls,
+                skip_plot=True,
             )
             nlp.plot[f"{var_names_with_suffix[-1]}_controls"] = CustomPlot(
                 lambda t0, phases_dt, node_idx, x, u, p, a, d, key: (
@@ -428,7 +427,9 @@ def _manage_fatigue_to_new_variable(
 
         for p, params in enumerate(fatigue_suffix):
             name_tp = f"{var_names_with_suffix[-1]}_{params}"
-            NewVariableConfiguration(name_tp, name_elements, ocp, nlp, True, False, skip_plot=True)
+            NewVariableConfiguration(
+                name_tp, name_elements, ocp, nlp, as_states=True, as_controls=False, skip_plot=True
+            )
             nlp.plot[name_tp] = CustomPlot(
                 lambda t0, phases_dt, node_idx, x, u, p, a, d, key, mod: (
                     mod * x[nlp.states.key_index(key), :] if x.any() else np.ndarray((len(name_elements), 1)) * np.nan