fine tuned bounds and constraints for nmpc

cocofest/models/ding2007_with_fatigue.py

@@ -55,6 +55,7 @@ def __init__(
         )
         self._with_fatigue = True
         self.stim_time = stim_time
+        self.fmax = 315
 
         # --- Default values --- #
         ALPHA_A_DEFAULT = -4.0 * 10e-2  # Value from Ding's experimentation [1] (s^-2)

cocofest/optimization/fes_ocp_multibody.py

@@ -134,7 +134,7 @@ def set_x_bounds_fes(bio_models):
                 if variable_bound_list[i] == "Cn_" + muscle_name:
                     max_bounds[i] = 10
                 elif variable_bound_list[i] == "F_" + muscle_name:
-                    max_bounds[i] = 1000
+                    max_bounds[i] = model.fmax
                 elif variable_bound_list[i] == "Tau1_" + muscle_name or variable_bound_list[i] == "Km_" + muscle_name:
                     max_bounds[i] = 1
                 elif variable_bound_list[i] == "A_" + muscle_name:

examples/fes_multibody/cycling/cycling_pulse_width_nmpc.py

@@ -98,13 +98,15 @@ def prepare_nmpc(
     x_bounds, x_init = set_bounds(
         model=model,
         x_init=x_init,
+        n_shooting=window_n_shooting,
+        turn_number=turn_number,
     )
 
     # Control path constraint
     u_bounds, u_init, u_scaling = set_u_bounds_and_init(model)
 
     # Constraints
-    constraints = set_constraints(model)
+    constraints = set_constraints(model, window_n_shooting, turn_number)
 
     # Update model
     model = updating_model(model=model, external_force_set=external_force_set, parameters=ParameterList(use_sx=use_sx))
@@ -204,10 +206,11 @@ def set_x_init(n_shooting, pedal_config, turn_number):
         x_center=pedal_config["x_center"],
         y_center=pedal_config["y_center"],
         radius=pedal_config["radius"],
-        ik_method="trf",
+        ik_method="lm",
         cycling_number=turn_number,
     )
     x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME)
+    x_init.add("qdot", [0, 0, -6], interpolation=InterpolationType.CONSTANT)
     # x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME)
 
     return x_init
@@ -226,23 +229,47 @@ def set_u_bounds_and_init(bio_model):
     )
 
 
-def set_bounds(model, x_init):
+def set_bounds(model, x_init, n_shooting, turn_number):
     x_bounds, x_init_fes = OcpFesMsk.set_x_bounds_fes(model)
     for key in x_init_fes.keys():
         x_init[key] = x_init_fes[key]
 
     # Retrieve default bounds from the model for positions and velocities
     q_x_bounds = model.bounds_from_ranges("q")
-    q_x_bounds.min[0] = [-0.5] * 3
-    q_x_bounds.max[0] = [1.5] * 3
-    q_x_bounds.min[1] = [1] * 3
-    q_x_bounds.min[2][0] = x_init["q"].init[2][0]
-    q_x_bounds.max[2][0] = x_init["q"].init[2][0]
-    q_x_bounds.min[2][-1] = x_init["q"].init[2][-1]
-    q_x_bounds.max[2][-1] = x_init["q"].init[2][-1]
-    q_x_bounds.max[2][1] = x_init["q"].init[2][0]
-    q_x_bounds.min[2][1] = x_init["q"].init[2][-1]
-    x_bounds.add(key="q", bounds=q_x_bounds, phase=0)
+
+    x_min_bound = []
+    x_max_bound = []
+    for i in range(q_x_bounds.min.shape[0]):
+        x_min_bound.append([q_x_bounds.min[i][0]] * (n_shooting + 1))
+        x_max_bound.append([q_x_bounds.max[i][0]] * (n_shooting + 1))
+
+    slack = 0.2
+    for i in range(len(x_min_bound[0])):
+        x_min_bound[0][i] = -0.5
+        x_max_bound[0][i] = 1.5
+        x_min_bound[1][i] = 1
+        x_min_bound[2][i] = x_init["q"].init[2][-1] - slack
+        x_max_bound[2][i] = x_init["q"].init[2][0] + slack
+
+    # Adjust bounds at cardinal nodes for a specific coordinate (e.g., index 2)
+    cardinal_node_list = [
+        i * (n_shooting / ((n_shooting / (n_shooting / turn_number)) * 1))
+        for i in range(int((n_shooting / (n_shooting / turn_number)) * 1 + 1))
+    ]
+    cardinal_node_list = [int(cardinal_node_list[i]) for i in range(len(cardinal_node_list))]
+
+    x_min_bound[2][0] = x_init["q"].init[2][0]
+    x_max_bound[2][0] = x_init["q"].init[2][0]
+    x_min_bound[2][-1] = x_init["q"].init[2][-1]
+    x_max_bound[2][-1] = x_init["q"].init[2][-1]
+
+    for i in range(1, len(cardinal_node_list) - 1):
+        x_max_bound[2][cardinal_node_list[i]] = x_init["q"].init[2][cardinal_node_list[i]] + slack
+        x_min_bound[2][cardinal_node_list[i]] = x_init["q"].init[2][cardinal_node_list[i]] - slack
+
+    x_bounds.add(
+        key="q", min_bound=x_min_bound, max_bound=x_max_bound, phase=0, interpolation=InterpolationType.EACH_FRAME
+    )
 
     # Modify bounds for velocities (e.g., setting maximum pedal speed to 0 to prevent the pedal to go backward)
     qdot_x_bounds = model.bounds_from_ranges("qdot")
@@ -256,7 +283,7 @@ def set_bounds(model, x_init):
     return x_bounds, x_init
 
 
-def set_constraints(bio_model):
+def set_constraints(bio_model, n_shooting, turn_number):
     constraints = ConstraintList()
     constraints.add(
         ConstraintFcn.TRACK_MARKERS_VELOCITY,
@@ -273,13 +300,21 @@ def set_constraints(bio_model):
         axes=[Axis.X, Axis.Y],
     )
 
-    constraints.add(
-        ConstraintFcn.SUPERIMPOSE_MARKERS,
-        first_marker="wheel_center",
-        second_marker="global_wheel_center",
-        node=33,
-        axes=[Axis.X, Axis.Y],
-    )
+    # Adjust bounds at cardinal nodes for a specific coordinate (e.g., index 2)
+    cardinal_node_list = [
+        i * (n_shooting / ((n_shooting / (n_shooting / turn_number)) * 1))
+        for i in range(int((n_shooting / (n_shooting / turn_number)) * 1 + 1))
+    ]
+    cardinal_node_list = [int(cardinal_node_list[i]) for i in range(len(cardinal_node_list))]
+
+    for i in range(1, len(cardinal_node_list) - 1):
+        constraints.add(
+            ConstraintFcn.SUPERIMPOSE_MARKERS,
+            first_marker="wheel_center",
+            second_marker="global_wheel_center",
+            node=cardinal_node_list[i],
+            axes=[Axis.X, Axis.Y],
+        )
 
     return constraints
 
@@ -322,11 +357,9 @@ def main():
     # NMPC parameters
     cycle_duration = 1
     n_cycles_to_advance = 1
-    n_cycles_simultaneous = 3
     n_cycles = 3
 
     # Bike parameters
-    turn_number = n_cycles_simultaneous
     pedal_config = {"x_center": 0.35, "y_center": 0.0, "radius": 0.1}
 
     resistive_torque = {

examples/fes_multibody/cycling/cycling_with_different_driven_methods.py

@@ -212,12 +212,13 @@ def set_x_init(n_shooting: int, pedal_config: dict, turn_number: int) -> Initial
         x_center=pedal_config["x_center"],
         y_center=pedal_config["y_center"],
         radius=pedal_config["radius"],
-        ik_method="trf",
+        ik_method="lm",
         cycling_number=turn_number,
     )
     x_init.add("q", q_guess, interpolation=InterpolationType.EACH_FRAME)
-    # x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME)
+    x_init.add("qdot", [0, 0, -6], interpolation=InterpolationType.CONSTANT)
 
+    # x_init.add("qdot", qdot_guess, interpolation=InterpolationType.EACH_FRAME)
     # Optional, method to get control initial guess:
     # u_guess = inverse_dynamics_cycling(biorbd_model_path, q_guess, qdot_guess, qddotguess)
     # u_init.add("tau", u_guess, interpolation=InterpolationType.EACH_FRAME)
@@ -270,6 +271,8 @@ def set_u_bounds_and_init(
 def set_state_bounds(
     model: BiorbdModel | FesMskModel,
     x_init: InitialGuessList,
+    n_shooting: int,
+    turn_number: int,
 ) -> tuple[BoundsList, InitialGuessList]:
     """
     Set the bounds for the state variables.
@@ -294,16 +297,40 @@ def set_state_bounds(
 
     # Retrieve default bounds from the model for positions and velocities
     q_x_bounds = model.bounds_from_ranges("q")
-    q_x_bounds.min[0] = [-0.5] * 3
-    q_x_bounds.max[0] = [1.5] * 3
-    q_x_bounds.min[1] = [1] * 3
-    q_x_bounds.min[2][0] = x_init["q"].init[2][0]
-    q_x_bounds.max[2][0] = x_init["q"].init[2][0]
-    q_x_bounds.min[2][-1] = x_init["q"].init[2][-1]
-    q_x_bounds.max[2][-1] = x_init["q"].init[2][-1]
-    q_x_bounds.max[2][1] = x_init["q"].init[2][0]
-    q_x_bounds.min[2][1] = x_init["q"].init[2][-1]
-    x_bounds.add(key="q", bounds=q_x_bounds, phase=0)
+
+    x_min_bound = []
+    x_max_bound = []
+    for i in range(q_x_bounds.min.shape[0]):
+        x_min_bound.append([q_x_bounds.min[i][0]] * (n_shooting + 1))
+        x_max_bound.append([q_x_bounds.max[i][0]] * (n_shooting + 1))
+
+    slack = 0.2
+    for i in range(len(x_min_bound[0])):
+        x_min_bound[0][i] = -0.5
+        x_max_bound[0][i] = 1.5
+        x_min_bound[1][i] = 1
+        x_min_bound[2][i] = x_init["q"].init[2][-1] - slack
+        x_max_bound[2][i] = x_init["q"].init[2][0] + slack
+
+    # Adjust bounds at cardinal nodes for a specific coordinate (e.g., index 2)
+    cardinal_node_list = [
+        i * (n_shooting / ((n_shooting / (n_shooting / turn_number)) * 1))
+        for i in range(int((n_shooting / (n_shooting / turn_number)) * 1 + 1))
+    ]
+    cardinal_node_list = [int(cardinal_node_list[i]) for i in range(len(cardinal_node_list))]
+
+    x_min_bound[2][0] = x_init["q"].init[2][0]
+    x_max_bound[2][0] = x_init["q"].init[2][0]
+    x_min_bound[2][-1] = x_init["q"].init[2][-1]
+    x_max_bound[2][-1] = x_init["q"].init[2][-1]
+
+    for i in range(1, len(cardinal_node_list) - 1):
+        x_max_bound[2][cardinal_node_list[i]] = x_init["q"].init[2][cardinal_node_list[i]] + slack
+        x_min_bound[2][cardinal_node_list[i]] = x_init["q"].init[2][cardinal_node_list[i]] - slack
+
+    x_bounds.add(
+        key="q", min_bound=x_min_bound, max_bound=x_max_bound, phase=0, interpolation=InterpolationType.EACH_FRAME
+    )
 
     # Modify bounds for velocities (e.g., setting maximum pedal speed to 0 to prevent the pedal to go backward)
     qdot_x_bounds = model.bounds_from_ranges("qdot")
@@ -317,7 +344,7 @@ def set_state_bounds(
     return x_bounds, x_init
 
 
-def set_constraints(model: BiorbdModel | FesMskModel) -> ConstraintList:
+def set_constraints(model: BiorbdModel | FesMskModel, n_shooting, turn_number) -> ConstraintList:
     """
     Set constraints for the optimal control problem.
 
@@ -346,13 +373,21 @@ def set_constraints(model: BiorbdModel | FesMskModel) -> ConstraintList:
         axes=[Axis.X, Axis.Y],
     )
 
-    constraints.add(
-        ConstraintFcn.SUPERIMPOSE_MARKERS,
-        first_marker="wheel_center",
-        second_marker="global_wheel_center",
-        node=33,
-        axes=[Axis.X, Axis.Y],
-    )
+    # Adjust bounds at cardinal nodes for a specific coordinate (e.g., index 2)
+    cardinal_node_list = [
+        i * (n_shooting / ((n_shooting / (n_shooting / turn_number)) * 1))
+        for i in range(int((n_shooting / (n_shooting / turn_number)) * 1 + 1))
+    ]
+    cardinal_node_list = [int(cardinal_node_list[i]) for i in range(len(cardinal_node_list))]
+
+    for i in range(1, len(cardinal_node_list) - 1):
+        constraints.add(
+            ConstraintFcn.SUPERIMPOSE_MARKERS,
+            first_marker="wheel_center",
+            second_marker="global_wheel_center",
+            node=cardinal_node_list[i],
+            axes=[Axis.X, Axis.Y],
+        )
 
     return constraints
 
@@ -416,13 +451,15 @@ def prepare_ocp(
     x_bounds, x_init = set_state_bounds(
         model=model,
         x_init=x_init,
+        n_shooting=n_shooting,
+        turn_number=turn_number,
     )
 
     # Define control bounds and initial guess
     u_init, u_bounds, u_scaling = set_u_bounds_and_init(model, dynamics_type_str=dynamics_type)
 
     # Set constraints
-    constraints = set_constraints(model)
+    constraints = set_constraints(model, n_shooting=n_shooting, turn_number=turn_number)
 
     # Update the model with external forces and parameters
     model = update_model(model, external_force_set, parameters=ParameterList(use_sx=use_sx))
@@ -438,7 +475,7 @@ def prepare_ocp(
         u_init=u_init,
         u_scaling=u_scaling,
         objective_functions=objective_functions,
-        # ode_solver=OdeSolver.RK4(n_integration_steps=integration_step),
+        # ode_solver=OdeSolver.RK1(n_integration_steps=integration_step),
         ode_solver=OdeSolver.COLLOCATION(polynomial_degree=2, method="radau"),
         n_threads=20,
         constraints=constraints,
@@ -488,7 +525,7 @@ def main():
         )
         # Adjust n_shooting based on the stimulation time
         n_shooting = model.muscles_dynamics_model[0].get_n_shooting(final_time)
-        integration_step = 10
+        integration_step = 5
     else:
         raise ValueError(f"Dynamics type '{dynamics_type}' not recognized")