[BREAKING] fix and update: holonomic constraint biais was wrong.

README.md

@@ -30,7 +30,7 @@ The current status of `bioptim` on conda-forge is
 You can join us on Discord 
 [![Discord](https://img.shields.io/discord/1340640457327247460.svg?label=chat&logo=discord&color=7289DA)](https://discord.gg/Ux7BkdjQFW)
 or open an Issue on GitHub.
-We would be trilled to discuss with you about `bioptim` and biomechanics/optimal control in general!
+We would be thrilled to discuss with you about `bioptim` and biomechanics/optimal control in general!
 
 
 # Try bioptim
@@ -264,7 +264,7 @@ conda install -c conda-forge bioptim
 ```
 This will download and install all the dependencies and install `bioptim`. 
 And that is it! 
-You can already enjoy bioptiming!
+You can already enjoy using bioptim!
 
 ## Installing from the sources (For Linux, Mac, and Windows)
 Installing from the sources is as easy as installing from Anaconda, with the difference that you will be required to download and install the dependencies by hand (see the section below). 

bioptim/dynamics/dynamics_functions.py

@@ -250,8 +250,8 @@ def get_contact_defects(nlp, q: CX, qdot: CX, slope_qdot: CX):
         contact_defects = nlp.cx()
         # We append the defects with the algebraic states implicit constraints
         if ContactType.RIGID_IMPLICIT in nlp.model.contact_types:
-            _, _, acceleration_constraint_func = HolonomicConstraintsFcn.rigid_contacts(nlp.model)
-            contact_acceleration_defect = acceleration_constraint_func(q, qdot, slope_qdot, nlp.parameters.cx)
+            _, jacobian, bias = HolonomicConstraintsFcn.rigid_contacts(nlp.model)
+            contact_acceleration_defect = jacobian(q, nlp.parameters.cx) @ slope_qdot + bias(q, qdot, nlp.parameters.cx)
             contact_defects = vertcat(contact_defects, contact_acceleration_defect)
 
         if ContactType.SOFT_IMPLICIT in nlp.model.contact_types:

bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup.py

@@ -1,11 +1,10 @@
 """
 This example presents how to implement a holonomic constraint in bioptim.
-The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double
-pendulum simulation.
+The simulation consists of an arm model with a pendulum attached, where the connection is enforced
+through a holonomic constraint.
 """
 
 import numpy as np
-from casadi import DM
 
 from bioptim import (
     BiMappingList,
@@ -28,34 +27,6 @@
 from bioptim.examples.utils import ExampleUtils
 
 
-def compute_all_states(sol, bio_model: HolonomicTorqueBiorbdModel):
-    """
-    Compute all the states from the solution of the optimal control program
-
-    Parameters
-    ----------
-    bio_model: HolonomicTorqueBiorbdModel
-        The biorbd model
-    sol:
-        The solution of the optimal control program
-
-    Returns
-    -------
-
-    """
-
-    states = sol.decision_states(to_merge=SolutionMerge.NODES)
-    n = states["q_u"].shape[1]
-    q = np.zeros((bio_model.nb_q, n))
-    q_v_init = DM.zeros(bio_model.nb_dependent_joints, n)
-
-    for i in range(n):
-        q_v_i = bio_model.compute_q_v()(states["q_u"][:, i], q_v_init[:, i]).toarray()
-        q[:, i] = bio_model.state_from_partition(states["q_u"][:, i][:, np.newaxis], q_v_i).toarray().squeeze()
-
-    return q
-
-
 def prepare_ocp(
     biorbd_model_path: str,
     n_shooting: int = 30,
@@ -181,7 +152,8 @@ def main():
     print(sol.decision_states(to_merge=SolutionMerge.NODES)["q_u"])
 
     # --- Show results --- #
-    q = compute_all_states(sol, bio_model)
+    states = sol.decision_states(to_merge=SolutionMerge.NODES)
+    q = bio_model.compute_q_from_u_iterative(states["q_u"], q)
 
     viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze())
     viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q)

bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle.py

@@ -1,11 +1,22 @@
 """
-This example presents how to implement a holonomic constraint in bioptim.
-The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double
-pendulum simulation.
+This example presents how to implement a holonomic constraint in bioptim with muscle-driven dynamics.
+The simulation is an arm with a pendulum attached, where the pendulum attachment is enforced through holonomic
+constraints. The dynamics are partitioned into independent (q_u) and dependent (q_v) coordinates, with q_v
+computed implicitly within the dynamics.
+
+Methods used from HolonomicBiorbdModel:
+---------------------------------------
+- compute_q_from_u_iterative(q_u_array, q_v_init=None):
+    Reconstructs the full generalized coordinates q from independent coordinates q_u.
+    Uses a Newton solver to iteratively compute the dependent coordinates q_v that satisfy
+    the holonomic constraints Φ(q_u, q_v) = 0. Optional warm-starting with q_v_init improves
+    convergence for subsequent time steps.
+
+Note: In this non-algebraic formulation, q_v is computed implicitly within the dynamics and is not
+part of the state vector. See arm26_pendulum_swingup_muscle_algebraic.py for the algebraic variant
+where q_v is included as algebraic states.
 """
 
-import numpy as np
-
 from bioptim import (
     BiMappingList,
     BoundsList,
@@ -14,7 +25,6 @@
     DynamicsOptionsList,
     HolonomicConstraintsFcn,
     HolonomicConstraintsList,
-    HolonomicTorqueBiorbdModel,
     InitialGuessList,
     ObjectiveFcn,
     ObjectiveList,
@@ -24,9 +34,12 @@
     Solver,
 )
 from bioptim.examples.utils import ExampleUtils
+import numpy as np
 
-from .arm26_pendulum_swingup import compute_all_states
-from .custom_dynamics import HolonomicMusclesBiorbdModel
+try:
+    from .custom_dynamics import HolonomicMusclesBiorbdModel
+except ImportError:
+    from custom_dynamics import HolonomicMusclesBiorbdModel
 
 
 def prepare_ocp(
@@ -78,6 +91,7 @@ def prepare_ocp(
     # Add objective functions
     objective_functions = ObjectiveList()
     objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=1, multi_thread=False)
+    objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=200, multi_thread=False)
 
     # Dynamics
     dynamics = DynamicsOptionsList()
@@ -93,35 +107,34 @@ def prepare_ocp(
     # The dynamics of the dependent joints will be computed from the holonomic constraint
     variable_bimapping.add("q", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4])
     variable_bimapping.add("qdot", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4])
+
     x_bounds = BoundsList()
-    # q_u and qdot_u are the states of the independent joints
+    # q_u and qdot_u are the states of the independent joints which are decision variables of the OCP
     x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping)
     x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping)
 
     x_bounds["q_u"][0, 0] = 0
     x_bounds["q_u"][1, 0] = np.pi / 2
     x_bounds["q_u"][2, 0] = 0
     x_bounds["q_u"][2, -1] = -np.pi
-    x_bounds["qdot_u"][:, [0, -1]] = 0  # Start and end without any velocity
+    x_bounds["qdot_u"][:, 0] = 0  # Start and end without any velocity
 
     # Initial guess
     x_init = InitialGuessList()
     x_init["q_u"] = [0.2] * 3
 
     # Define control path constraint
-    tau_min, tau_max, tau_init = -100, 100, 0
-    # Only the rotations are controlled
-    variable_bimapping.add("tau", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4])
+
     u_bounds = BoundsList()
-    u_bounds.add("tau", min_bound=[tau_min] * 3, max_bound=[tau_max] * 3)
     u_bounds.add("muscles", min_bound=[0] * 6, max_bound=[1] * 6)
-    u_bounds["tau"][:, :] = 0
-    u_bounds["tau"][-1, :] = 0
-    u_init = InitialGuessList()
-    # u_init.add("tau", [tau_init] * 2)
 
-    # ------------- #
+    tau_min, tau_max, tau_init = -3, 3, 0  # Residual torques
+    u_bounds.add("tau", min_bound=[tau_min] * 3, max_bound=[tau_max] * 3)
+
+    u_init = InitialGuessList()
 
+    # Only the rotations are controlled, not the translations, which are constrained by the holonomic constraint
+    variable_bimapping.add("tau", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4])
     return (
         OptimalControlProgram(
             bio_model,
@@ -148,21 +161,22 @@ def main():
     import pyorerun
 
     model_path = ExampleUtils.folder + "/models/arm26_w_pendulum.bioMod"
-    ocp, bio_model = prepare_ocp(biorbd_model_path=model_path)
+    ocp, bio_model = prepare_ocp(
+        biorbd_model_path=model_path,
+        final_time=0.5,
+        n_shooting=10,
+    )
 
     # --- Solve the program --- #
-    sol = ocp.solve(Solver.IPOPT(show_online_optim=False))
-    print(sol.real_time_to_optimize)
-
-    print(sol.decision_states(to_merge=SolutionMerge.NODES)["q_u"])
+    sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
 
     # --- Show results --- #
-    q = compute_all_states(sol, bio_model)
+    states = sol.decision_states(to_merge=SolutionMerge.NODES)
+    q = bio_model.compute_q_from_u_iterative(states["q_u"])
 
     viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze())
     viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q)
-
-    viz.rerun("double_pendulum")
+    viz.rerun()
 
     sol.graphs()
 

bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle_algebraic.py

@@ -1,7 +1,19 @@
 """
-This example presents how to implement a holonomic constraint in bioptim.
-The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double
-pendulum simulation.
+This example presents how to implement a holonomic constraint in bioptim with algebraic states.
+The simulation is an arm with a pendulum attached, where the pendulum attachment is enforced through holonomic
+constraints. Unlike the non-algebraic version, this implementation uses algebraic states (q_v) for dependent
+coordinates, requiring explicit constraint enforcement at each node.
+
+Methods used from HolonomicBiorbdModel:
+---------------------------------------
+- compute_q_from_u_iterative(q_u_array, q_v_init=None):
+    Reconstructs the full generalized coordinates q from independent coordinates q_u.
+    In the algebraic version, q_v_init is provided from the algebraic states to warm-start
+    the iterative solver, improving convergence and ensuring consistency with the OCP solution.
+
+Note: The algebraic formulation explicitly includes q_v as algebraic states in the OCP, which are
+constrained through path constraints (constraint_holonomic). This differs from the non-algebraic
+version where q_v is implicitly computed within the dynamics.
 """
 
 import numpy as np
@@ -23,11 +35,14 @@
     OptimalControlProgram,
     SolutionMerge,
     Solver,
+    CostType,
 )
 from bioptim.examples.utils import ExampleUtils
 
-from .arm26_pendulum_swingup import compute_all_states
-from .custom_dynamics import AlgebraicHolonomicMusclesBiorbdModel, constraint_holonomic, constraint_holonomic_end
+try:
+    from .custom_dynamics import AlgebraicHolonomicMusclesBiorbdModel, constraint_holonomic, constraint_holonomic_end
+except ImportError:
+    from custom_dynamics import AlgebraicHolonomicMusclesBiorbdModel, constraint_holonomic, constraint_holonomic_end
 
 
 def prepare_ocp(
@@ -80,6 +95,7 @@ def prepare_ocp(
     # Add objective functions
     objective_functions = ObjectiveList()
     objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=1, multi_thread=False)
+    objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=200, multi_thread=False)
 
     # Dynamics
     dynamics = DynamicsOptionsList()
@@ -105,7 +121,7 @@ def prepare_ocp(
     x_bounds["q_u"][1, 0] = np.pi / 2
     x_bounds["q_u"][2, 0] = 0
     x_bounds["q_u"][2, -1] = -np.pi
-    x_bounds["qdot_u"][:, [0, -1]] = 0  # Start and end without any velocity
+    x_bounds["qdot_u"][:, 0] = 0  # Start without any velocity
 
     a_bounds = BoundsList()
     a_bounds.add("q_v", bio_model.bounds_from_ranges("q", mapping=v_variable_bimapping))
@@ -115,17 +131,13 @@ def prepare_ocp(
     x_init["q_u"] = [0.2] * 3
 
     # Define control path constraint
-    tau_min, tau_max, tau_init = -100, 100, 0
-    # Only the rotations are controlled
-    tau_variable_bimapping = BiMappingList()
-    tau_variable_bimapping.add("tau", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4])
+
     u_bounds = BoundsList()
-    u_bounds.add("tau", min_bound=[tau_min] * 3, max_bound=[tau_max] * 3)
     u_bounds.add("muscles", min_bound=[0] * 6, max_bound=[1] * 6)
-    u_bounds["tau"][:, :] = 0
-    u_bounds["tau"][-1, :] = 0
+
+    tau_min, tau_max, tau_init = -3, 3, 0  # Residual torques
+    u_bounds.add("tau", min_bound=[tau_min] * 3, max_bound=[tau_max] * 3)
     u_init = InitialGuessList()
-    # u_init.add("tau", [tau_init] * 2)
 
     # Path Constraints
     constraints = ConstraintList()
@@ -140,6 +152,10 @@ def prepare_ocp(
 
     # ------------- #
 
+    # Only the rotations are controlled, not the translations, which are constrained by the holonomic constraint
+    tau_variable_bimapping = BiMappingList()
+    tau_variable_bimapping.add("tau", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4])
+
     return (
         OptimalControlProgram(
             bio_model,
@@ -148,6 +164,7 @@ def prepare_ocp(
             dynamics=dynamics,
             x_bounds=x_bounds,
             u_bounds=u_bounds,
+            a_bounds=a_bounds,
             x_init=x_init,
             u_init=u_init,
             objective_functions=objective_functions,
@@ -167,24 +184,27 @@ def main():
     import pyorerun
 
     model_path = ExampleUtils.folder + "/models/arm26_w_pendulum.bioMod"
-    ocp, bio_model = prepare_ocp(biorbd_model_path=model_path)
+    ocp, bio_model = prepare_ocp(
+        biorbd_model_path=model_path,
+        final_time=0.5,
+        n_shooting=10,
+    )
 
     # --- Solve the program --- #
-    sol = ocp.solve(Solver.IPOPT(show_online_optim=False))
-    print(sol.real_time_to_optimize)
-
-    print(sol.decision_states(to_merge=SolutionMerge.NODES)["q_u"])
+    sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
 
     # --- Show results --- #
-    q = compute_all_states(sol, bio_model)
+    stepwise_q_u = sol.stepwise_states(to_merge=SolutionMerge.NODES)["q_u"]
+    stepwise_q_v = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES)["q_v"]
+    q = ocp.nlp[0].model.state_from_partition(stepwise_q_u, stepwise_q_v).toarray()
 
     viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze())
     viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q)
-
-    viz.rerun("double_pendulum")
+    viz.rerun()
 
     sol.graphs()
 
 
 if __name__ == "__main__":
     main()
+    main()