diff --git a/README.md b/README.md index 0ab029089..d8d4e207c 100644 --- a/README.md +++ b/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). diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index e21900f0b..bc483d782 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/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: diff --git a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup.py b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup.py index ee8cf9069..528ba0265 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup.py +++ b/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"]) viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) diff --git a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle.py b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle.py index 55ed33693..021a3279d 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle.py +++ b/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,8 +107,9 @@ 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) @@ -102,26 +117,24 @@ 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 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() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle_algebraic.py b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle_algebraic.py index ba755abd5..ab74aae81 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle_algebraic.py +++ b/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,21 +184,23 @@ 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() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/common.py b/bioptim/examples/toy_examples/holonomic_constraints/common.py deleted file mode 100644 index 224aa222c..000000000 --- a/bioptim/examples/toy_examples/holonomic_constraints/common.py +++ /dev/null @@ -1,34 +0,0 @@ -import numpy as np -from casadi import DM - -from bioptim import HolonomicTorqueBiorbdModel, SolutionMerge - - -def compute_all_q(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 diff --git a/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py b/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py index 3bd5eb734..a1a211b2c 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py @@ -21,9 +21,6 @@ ParameterList, PenaltyController, States, - DefectType, - TorqueDynamics, - MusclesDynamics, ) @@ -193,8 +190,6 @@ def get_basic_variables( ): # Get variables from the right place - # q = DynamicsFunctions.get(nlp.states["q"], states) - # qdot = DynamicsFunctions.get(nlp.states["qdot"], states) q_u = DynamicsFunctions.get(nlp.states["q_u"], states) qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) q_v_init = DM.zeros(nlp.model.nb_dependent_joints) @@ -229,9 +224,6 @@ def dynamics( nlp, ): - # q_u = DynamicsFunctions.get(nlp.states["q_u"], states) - # qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) - # tau = DynamicsFunctions.get(nlp.controls["tau"], controls) # Get torques from muscles + residual torques q_u, qdot_u, tau, _, _ = self.get_basic_variables( nlp, states, controls, parameters, algebraic_states, numerical_timeseries ) diff --git a/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py index e756f2f0a..1f604f925 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py @@ -1,7 +1,6 @@ """ 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 a four-bar linkage mechanism constrained by holonomic constraints. """ import numpy as np @@ -24,8 +23,6 @@ from bioptim.examples.utils import ExampleUtils -from .common import compute_all_q - def prepare_ocp( biorbd_model_path: str, @@ -167,7 +164,8 @@ def main(): print(sol.real_time_to_optimize) # --- Show results --- # - q = compute_all_q(sol, bio_model) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + q = bio_model.compute_q_from_u_iterative(states["q_u"]) viewer = "pyorerun" if viewer == "bioviz": diff --git a/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py b/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py index 4c1d7fb28..32ed8d03f 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py @@ -1,7 +1,6 @@ """ 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 a three-bar linkage mechanism constrained by holonomic constraints. """ import numpy as np @@ -23,8 +22,6 @@ ) from bioptim.examples.utils import ExampleUtils -from .common import compute_all_q - def prepare_ocp( biorbd_model_path: str, @@ -153,7 +150,8 @@ def main(): # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) - q = compute_all_q(sol, bio_model) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + q = bio_model.compute_q_from_u_iterative(states["q_u"]) viewer = "pyorerun" if viewer == "bioviz": diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py index c0e0c1baa..49d72f60d 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py @@ -1,7 +1,7 @@ """ 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 two single pendulums that are forced to be coherent with a holonomic constraint, +creating a double pendulum simulation. """ from bioptim import ( @@ -22,67 +22,9 @@ Solver, ) from bioptim.examples.utils import ExampleUtils -from casadi import DM import numpy as np -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) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - - n = states["q_u"].shape[1] - n_tau = controls["tau"].shape[1] - - q = np.zeros((bio_model.nb_q, n)) - qdot = np.zeros((bio_model.nb_q, n)) - qddot = np.zeros((bio_model.nb_q, n)) - lambdas = np.zeros((bio_model.nb_dependent_joints, n)) - tau = np.zeros((bio_model.nb_tau, n_tau + 1)) - - for i, independent_joint_index in enumerate(bio_model.independent_joint_index): - tau[independent_joint_index, :-1] = controls["tau"][i, :] - for i, dependent_joint_index in enumerate(bio_model.dependent_joint_index): - tau[dependent_joint_index, :-1] = controls["tau"][i, :] - - 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() - qdot[:, i] = bio_model.compute_qdot()(q[:, i], states["qdot_u"][:, i]).toarray().squeeze() - qddot_u_i = ( - bio_model.partitioned_forward_dynamics()( - states["q_u"][:, i], states["qdot_u"][:, i], q_v_init[:, i], tau[:, i] - ) - .toarray() - .squeeze() - ) - qddot[:, i] = bio_model.compute_qddot()(q[:, i], qdot[:, i], qddot_u_i).toarray().squeeze() - lambdas[:, i] = ( - bio_model.compute_the_lagrangian_multipliers()( - states["q_u"][:, i][:, np.newaxis], states["qdot_u"][:, i], q_v_init[:, i], tau[:, i] - ) - .toarray() - .squeeze() - ) - - return q, qdot, qddot, lambdas - - def prepare_ocp( biorbd_model_path: str, n_shooting: int = 30, @@ -205,7 +147,16 @@ def main(): print(sol.real_time_to_optimize) # --- Show results --- # - q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + + # Prepare tau array for all joints + n_nodes = states["q_u"].shape[1] + tau = np.zeros((bio_model.nb_tau, n_nodes)) + tau[:, :-1] = controls["tau"] + + # Compute all states using the model method + q, qdot, qddot, lambdas = bio_model.compute_all_states_from_u_iterative(states["q_u"], states["qdot_u"], tau) viewer = "pyorerun" if viewer == "bioviz": diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py index 6c4e2dde1..07807e999 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py @@ -1,7 +1,6 @@ """ -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 multiple holonomic constraints in bioptim. +The simulation consists of two single pendulums with two holonomic constraints enforcing their connection. """ import numpy as np @@ -23,8 +22,6 @@ ) from bioptim.examples.utils import ExampleUtils -from .common import compute_all_q - def prepare_ocp( biorbd_model_path: str, @@ -82,7 +79,6 @@ def prepare_ocp( # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) - # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) # Dynamics dynamics = DynamicsOptionsList() @@ -153,7 +149,8 @@ def main(): print(sol.real_time_to_optimize) # --- Show results --- # - q = compute_all_q(sol, bio_model) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + q = bio_model.compute_q_from_u_iterative(states["q_u"]) viewer = "pyorerun" if viewer == "bioviz": diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py index 2cbd4dd2d..2476109ef 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py @@ -1,7 +1,6 @@ """ -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 multiple holonomic constraints in bioptim with 4 DOF. +The simulation consists of two single pendulums with two holonomic constraints. """ import numpy as np @@ -23,8 +22,6 @@ ) from bioptim.examples.utils import ExampleUtils -from .common import compute_all_q - def prepare_ocp( biorbd_model_path: str, @@ -82,7 +79,6 @@ def prepare_ocp( # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) - # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) # Dynamics dynamics = DynamicsOptionsList() @@ -150,7 +146,8 @@ def main(): print(sol.real_time_to_optimize) # --- Show results --- # - q = compute_all_q(sol, bio_model) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + q = bio_model.compute_q_from_u_iterative(states["q_u"]) viewer = "pyorerun" if viewer == "bioviz": diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py index 992d5d62f..737dd0aa3 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py @@ -1,41 +1,36 @@ """ -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. But this time, the dynamics are computed with the algebraic states, namely q_v the dependent joints +This example presents how to implement a holonomic constraint in bioptim with algebraic states. +The simulation consists of two single pendulums that are forced to be coherent with a holonomic constraint, +creating a double pendulum simulation. The dynamics are computed with algebraic states (q_v) representing +the dependent joints. """ from bioptim import ( BiMappingList, BoundsList, ConstraintList, - CostType, DynamicsOptions, DynamicsOptionsList, HolonomicConstraintsFcn, HolonomicConstraintsList, InitialGuessList, - Node, ObjectiveFcn, ObjectiveList, - OdeSolver, OptimalControlProgram, SolutionMerge, Node, CostType, OdeSolver, OnlineOptim, + Solver, ) from bioptim.examples.utils import ExampleUtils -from casadi import DM import numpy as np -from .custom_dynamics import ( - ModifiedHolonomicTorqueBiorbdModel, - constraint_holonomic, - constraint_holonomic_end, -) - -from .custom_dynamics import ModifiedHolonomicTorqueBiorbdModel, constraint_holonomic, constraint_holonomic_end +try: + from .custom_dynamics import ModifiedHolonomicTorqueBiorbdModel, constraint_holonomic, constraint_holonomic_end +except ImportError: + from custom_dynamics import ModifiedHolonomicTorqueBiorbdModel, constraint_holonomic, constraint_holonomic_end def prepare_ocp( @@ -44,7 +39,7 @@ def prepare_ocp( final_time: float = 1, expand_dynamics: bool = False, ode_solver: OdeSolver = OdeSolver.COLLOCATION(polynomial_degree=2), -) -> tuple[ModifiedHolonomicTorqueBiorbdModel, OptimalControlProgram]: +) -> OptimalControlProgram: """ Prepare the program @@ -156,23 +151,20 @@ def prepare_ocp( # penalty_type=PenaltyType.INTERNAL, ) - return ( - OptimalControlProgram( - bio_model, - n_shooting, - final_time, - dynamics=dynamics, - x_bounds=x_bounds, - u_bounds=u_bounds, - a_bounds=a_bounds, - x_init=x_init, - u_init=u_init, - a_init=a_init, - objective_functions=objective_functions, - variable_mappings=tau_variable_bimapping, - constraints=constraints, - ), + return OptimalControlProgram( bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + a_bounds=a_bounds, + x_init=x_init, + u_init=u_init, + a_init=a_init, + objective_functions=objective_functions, + variable_mappings=tau_variable_bimapping, + constraints=constraints, ) @@ -182,11 +174,11 @@ def main(): """ biorbd_model_path = ExampleUtils.folder + "/models/two_pendulums.bioMod" - ocp, bio_model = prepare_ocp(biorbd_model_path=biorbd_model_path) + ocp = prepare_ocp(biorbd_model_path=biorbd_model_path) ocp.add_plot_penalty(CostType.ALL) # --- Solve the program --- # - sol = ocp.solve(Solver.IPOPT(OnlineOptim.DEFAULT)) + sol = ocp.solve(Solver.IPOPT()) print(sol.real_time_to_optimize) stepwise_q_u = sol.stepwise_states(to_merge=SolutionMerge.NODES)["q_u"] diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py index 320e57efe..b0bb36af8 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py @@ -1,7 +1,6 @@ """ -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 a spherical joint (rotule). +The simulation consists of two single pendulums connected through a spherical joint constraint. """ import numpy as np @@ -23,8 +22,6 @@ ) from bioptim.examples.utils import ExampleUtils -from .common import compute_all_q - def prepare_ocp( biorbd_model_path: str, @@ -71,12 +68,10 @@ def prepare_ocp( independent_joint_index=[1], dependent_joint_index=[0, 2, 3], ) - # bio_model = TorqueBiorbdModel(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", multi_thread=False) - # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) # Dynamics dynamics = DynamicsOptionsList() @@ -147,7 +142,8 @@ def main(): sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) print(sol.real_time_to_optimize) - q = compute_all_q(sol, bio_model) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + q = bio_model.compute_q_from_u_iterative(states["q_u"]) viewer = "pyorerun" if viewer == "bioviz": diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index 65f9d68e5..07b8112b1 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -6,7 +6,7 @@ from biorbd_casadi import ( GeneralizedCoordinates, ) -from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv, nlpsol +from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv, nlpsol, jacobian from .biorbd_model import BiorbdModel from ...models.protocols.holonomic_constraints import HolonomicConstraintsList @@ -24,7 +24,152 @@ class HolonomicBiorbdModel(BiorbdModel): """ - This class allows to define a biorbd model with custom holonomic constraints. + A biomechanical model with holonomic constraints for constrained multibody dynamics. + + This class extends BiorbdModel to support custom holonomic constraints, enabling the + simulation and optimization of mechanical systems with geometric restrictions (e.g., + closed kinematic chains, contact constraints, or enforced alignments). + + The class provides two formulations for constrained dynamics: + + 1. **Full-coordinate formulation** (via Lagrange multipliers): + Solves: M(q)q̈ + h(q,q̇) = τ + Jᵀλ subject to Φ(q) = 0 + + 2. **Reduced-coordinate formulation** (partitioned dynamics): + Eliminates dependent coordinates by partitioning q = [q_u; q_v] where q_v + is determined by q_u through the constraints, resulting in a smaller system. + + Attributes + ---------- + _newton_tol : float, default=1e-10 + Convergence tolerance for Newton's method when solving constraint equations. + Used in compute_q_v() to find dependent coordinates from independent ones. + + _holonomic_constraints : list[Function] + List of CasADi Functions representing the constraint equations Φ(q) = 0. + Each function maps q → constraint_residual. + + _holonomic_constraints_jacobians : list[Function] + List of CasADi Functions for constraint Jacobians J(q) = ∂Φ/∂q. + Each function maps q → Jacobian_matrix. + + _holonomic_constraints_bias : list[Function] + List of CasADi Functions for constraint bias terms J̇q̇. + Each function maps (q, q̇) → bias_vector, representing velocity-dependent + accelerations computed via the Hessian method. + + stabilization : bool, default=False + Whether to enable Baumgarte constraint stabilization in forward dynamics. + When True, adds feedback terms to reduce constraint drift: + Φ̈ = Jq̈ + J̇q̇ + αΦ + βJ̇ = 0 + + alpha : float, default=0.01 + Baumgarte stabilization gain for position-level constraint errors. + Higher values increase stiffness but may cause numerical issues. + Only active when stabilization=True. + + beta : float, default=0.01 + Baumgarte stabilization gain for velocity-level constraint errors. + Acts as damping on constraint violations. + Only active when stabilization=True. + + _dependent_joint_index : list[int] + Indices of dependent (constrained) generalized coordinates q_v. + These coordinates are determined from independent coordinates via constraints. + Must be sorted in ascending order. + + _independent_joint_index : list[int] + Indices of independent (unconstrained) generalized coordinates q_u. + These are the minimal coordinates needed to describe the system state. + Must be sorted in ascending order. + Default: all coordinates [0, 1, ..., nb_q-1] if not partitioned. + + _cached_functions : dict + Internal cache for compiled CasADi Functions to avoid recomputation. + Populated by the @cache_function decorator. + + Parameters + ---------- + bio_model : str | biorbd.Model + Path to the bioMod file or a biorbd.Model instance. + friction_coefficients : np.ndarray, optional + Friction coefficients for contact dynamics (inherited from BiorbdModel). + parameters : ParameterList, optional + Model parameters (currently not supported with holonomic constraints). + **kwargs + Additional arguments passed to BiorbdModel.__init__(). + + Notes + ----- + - Constraints must be set via set_holonomic_configuration() before use + - The partitioning q = [q_u; q_v] is optional but required for reduced dynamics + - For reduced dynamics, J_v (dependent Jacobian block) must be invertible + - The class automatically verifies invertibility via check_dependant_jacobian() + + Mathematical Background + ----------------------- + Holonomic constraints are geometric restrictions of the form Φ(q) = 0. + Taking time derivatives: + - Velocity level: Φ̇ = J(q)q̇ = 0 + - Acceleration level: Φ̈ = J(q)q̈ + J̇(q)q̇ = 0 + + The bias term J̇q̇ is computed using the Hessian method: + (J̇q̇)ᵢ = Σⱼ Σₖ (∂Jᵢⱼ/∂qₖ) q̇ₖ q̇ⱼ = q̇ᵀ Hᵢ q̇ + + For partitioned dynamics, the coupling relationships are: + q̇_v = B_vu q̇_u where B_vu = -J_v⁻¹ J_u + q̈_v = B_vu q̈_u + b_v where b_v = -J_v⁻¹(J̇q̇) + + Examples + -------- + Basic setup with marker superimposition constraint: + + >>> from bioptim import HolonomicBiorbdModel, HolonomicConstraintsList, HolonomicConstraintsFcn + >>> + >>> model = HolonomicBiorbdModel("my_model.bioMod") + >>> + >>> # Define constraints + >>> constraints = HolonomicConstraintsList() + >>> constraints.add( + ... "marker_constraint", + ... constraints_fcn=HolonomicConstraintsFcn.superimpose_markers, + ... marker_1="hand", + ... marker_2="target" + ... ) + >>> + >>> # Configure with partitioning + >>> model.set_holonomic_configuration( + ... constraints_list=constraints, + ... independent_joint_index=[0, 1, 2], + ... dependent_joint_index=[3, 4, 5] + ... ) + + Using reduced-coordinate forward dynamics: + + >>> # Only need independent coordinates + >>> q_u = np.array([0.1, 0.2, 0.3]) + >>> qdot_u = np.array([0.01, 0.02, 0.03]) + >>> tau = np.zeros(model.nb_q) + >>> + >>> # Compute full state from independent coordinates + >>> q = model.compute_q()(q_u, np.zeros(3)) # q_v_init = zeros + >>> + >>> # Compute accelerations (reduced system) + >>> qddot_u = model.partitioned_forward_dynamics()(q_u, qdot_u, np.zeros(3), tau) + + See Also + -------- + BiorbdModel : Parent class for unconstrained biomechanical models + HolonomicConstraintsList : Container for defining multiple constraints + HolonomicConstraintsFcn : Library of predefined constraint types + + References + ---------- + .. [1] Docquier, N., Poncelet, A., and Fisette, P. (2013). + ROBOTRAN: a powerful symbolic generator of multibody models. + Mech. Sci., 4, 199–219. https://doi.org/10.5194/ms-4-199-2013 + .. [2] Baumgarte, J. (1972). Stabilization of constraints and integrals of motion + in dynamical systems. Computer Methods in Applied Mechanics and Engineering. """ def __init__( @@ -40,8 +185,7 @@ def __init__( self._newton_tol = 1e-10 self._holonomic_constraints = [] self._holonomic_constraints_jacobians = [] - self._holonomic_constraints_derivatives = [] - self._holonomic_constraints_double_derivatives = [] + self._holonomic_constraints_bias = [] self.stabilization = False self.alpha = 0.01 self.beta = 0.01 @@ -114,13 +258,11 @@ def set_holonomic_configuration( for constraints_name in constraints_list.keys(): constraint_fcn = constraints_list[constraints_name]["constraints_fcn"] extra_arguments = constraints_list[constraints_name]["extra_arguments"] - constraint, constraint_jacobian, constraint_double_derivative = constraint_fcn( - model=self, **extra_arguments - ) + constraint, constraint_jacobian, constraint_bias = constraint_fcn(model=self, **extra_arguments) self._add_holonomic_constraint( constraint, constraint_jacobian, - constraint_double_derivative, + constraint_bias, ) if dependent_joint_index and independent_joint_index: @@ -244,11 +386,11 @@ def _add_holonomic_constraint( self, constraints: Function | Callable[[GeneralizedCoordinates], MX], constraints_jacobian: Function | Callable[[GeneralizedCoordinates], MX], - constraints_double_derivative: Function | Callable[[GeneralizedCoordinates], MX], + constraints_bias: Function | Callable[[GeneralizedCoordinates], MX], ): self._holonomic_constraints.append(constraints) self._holonomic_constraints_jacobians.append(constraints_jacobian) - self._holonomic_constraints_double_derivatives.append(constraints_double_derivative) + self._holonomic_constraints_bias.append(constraints_bias) @property def nb_holonomic_constraints(self): @@ -265,13 +407,169 @@ def holonomic_constraints_jacobian(self, q: MX) -> MX: return vertcat(*[c(q) for c in self._holonomic_constraints_jacobians]) def holonomic_constraints_derivative(self, q: MX, qdot: MX) -> MX: + """ + Compute the time derivative of the holonomic constraints at the velocity level. + + For holonomic constraints Φ(q) = 0, this computes: + Φ̇ = J(q)q̇ + + where J(q) = ∂Φ/∂q is the constraint Jacobian. + + Parameters + ---------- + q : MX + Generalized coordinates, shape (n × 1). + qdot : MX + Generalized velocities, shape (n × 1). + + Returns + ------- + MX + Time derivative of constraints Φ̇, shape (m × 1) where m is the number of constraints. + """ return self.holonomic_constraints_jacobian(q) @ qdot - def holonomic_constraints_double_derivative(self, q: MX, qdot: MX, qddot: MX) -> MX: - return vertcat(*[c(q, qdot, qddot) for c in self._holonomic_constraints_double_derivatives]) + def holonomic_constraints_bias(self, q: MX, qdot: MX, parameters: MX = None) -> MX: + """ + Compute the bias vector J̇q̇ for the holonomic constraint acceleration equation. + + This method evaluates the velocity-dependent acceleration term that appears in the + second time derivative of the holonomic constraints: + Φ̈ = J(q)q̈ + J̇(q)q̇ = 0 + + The bias term J̇q̇ is computed using the Hessian method for each constraint. + + Mathematical Background + ----------------------- + For constraints Φ(q) = 0, the acceleration-level equation is: + d²Φ/dt² = (∂Φ/∂q)q̈ + d/dt(∂Φ/∂q)q̇ = Jq̈ + J̇q̇ = 0 + + The bias vector represents the quadratic velocity terms: + (J̇q̇)ᵢ = Σⱼ Σₖ (∂Jᵢⱼ/∂qₖ) q̇ₖ q̇ⱼ = q̇ᵀ Hᵢ q̇ + + where Hᵢ is the Hessian of the i-th constraint. + + Parameters + ---------- + q : MX + Generalized coordinates, shape (n × 1). + qdot : MX + Generalized velocities, shape (n × 1). + + Returns + ------- + MX + Bias vector J̇q̇, shape (m × 1) where m is the total number of holonomic constraints. + + See Also + -------- + holonomic_constraints_double_derivative : Full acceleration-level constraint equation + constrained_forward_dynamics : Uses this bias term in the constrained dynamics + bias_vector : Partitioned version used in reduced-coordinate formulation + """ + if parameters: + return vertcat(*[b(q, qdot, parameters) for b in self._holonomic_constraints_bias]) + else: + return vertcat(*[b(q, qdot) for b in self._holonomic_constraints_bias]) + + def holonomic_constraints_double_derivative(self, q: MX, qdot: MX, qddot: MX, parameters: MX = None) -> MX: + """ + Compute the second time derivative of the holonomic constraints (acceleration level). + + For holonomic constraints Φ(q) = 0, this computes: + Φ̈ = J(q)q̈ + J̇(q)q̇ + + where: + - J(q) = ∂Φ/∂q is the constraint Jacobian + - J̇q̇ is the bias vector (velocity-dependent acceleration term) + + This equation must equal zero for the constraints to be satisfied at the acceleration level, + which is enforced in constrained dynamics formulations. + + Parameters + ---------- + q : MX + Generalized coordinates, shape (n × 1). + qdot : MX + Generalized velocities, shape (n × 1). + qddot : MX + Generalized accelerations, shape (n × 1). + + Returns + ------- + MX + Second time derivative Φ̈ = Jq̈ + J̇q̇, shape (m × 1) where m is the number of constraints. + + See Also + -------- + holonomic_constraints_bias : Computes the J̇q̇ term + constrained_forward_dynamics : Enforces Φ̈ = 0 to solve for q̈ and constraint forces + """ + if parameters: + return vertcat( + *[ + J(q, parameters) @ qddot + b(q, qdot, parameters) + for J, b in zip(self._holonomic_constraints_jacobians, self._holonomic_constraints_bias) + ] + ) + else: + return vertcat( + *[ + J(q) @ qddot + Jdot_qdot(q, qdot) + for J, Jdot_qdot in zip(self._holonomic_constraints_jacobians, self._holonomic_constraints_bias) + ] + ) @cache_function def constrained_forward_dynamics(self) -> Function: + """ + Compute forward dynamics for a system with holonomic constraints using Lagrange multipliers. + + This method solves the constrained equations of motion: + M(q)q̈ + h(q,q̇) = τ + Jᵀλ + J(q)q̈ + J̇(q)q̇ = 0 + + where: + - M(q) is the mass/inertia matrix + - h(q,q̇) are the nonlinear effects (Coriolis, centrifugal, gravity) + - τ are the applied generalized forces + - J(q) is the constraint Jacobian + - λ are the Lagrange multipliers (constraint forces) + + Mathematical Formulation + ------------------------ + The augmented system is solved as a linear system: + [M Jᵀ] [q̈] [τ - h ] + [J 0 ] [λ] = [-J̇q̇ ] + + Optional Baumgarte stabilization can be enabled to reduce constraint drift: + [-J̇q̇ - αΦ - βJ̇] + + where α and β are stabilization gains. + + Returns + ------- + Function + CasADi Function with signature: + Inputs: q, qdot, tau, parameters + Output: qddot (generalized accelerations satisfying constraints) + + Notes + ----- + - The method uses symbolic QR decomposition for numerical stability + - Lagrange multipliers are not returned but can be computed separately + - For reduced-coordinate formulations, use partitioned_forward_dynamics instead + + See Also + -------- + partitioned_forward_dynamics : Reduced-coordinate formulation (more efficient) + holonomic_constraints_bias : Computes the J̇q̇ bias term + + References + ---------- + .. [1] Baumgarte, J. (1972). Stabilization of constraints and integrals of motion + in dynamical systems. Computer Methods in Applied Mechanics and Engineering. + """ mass_matrix = self.mass_matrix()(self.q, self.parameters) constraints_jacobian = self.holonomic_constraints_jacobian(self.q) @@ -290,13 +588,13 @@ def constrained_forward_dynamics(self) -> Function: # compute b vector tau_augmented = self.tau - self.non_linear_effects()(self.q, self.qdot, self.parameters) - biais = -self.holonomic_constraints_jacobian(self.qdot) @ self.qdot + bias = -self.holonomic_constraints_bias(self.q, self.qdot) if self.stabilization: - biais -= self.alpha * self.holonomic_constraints( - self.q - ) + self.beta * self.holonomic_constraints_derivative(self.q, self.qdot) + bias -= self.alpha * self.holonomic_constraints(self.q) + self.beta * self.holonomic_constraints_derivative( + self.q, self.qdot + ) - tau_augmented = vertcat(tau_augmented, biais) + tau_augmented = vertcat(tau_augmented, bias) # solve with casadi Ax = b x = solve(mass_matrix_augmented, tau_augmented, "symbolicqr") @@ -364,7 +662,7 @@ def partitioned_forward_dynamics(self) -> Function: Sources ------- Docquier, N., Poncelet, A., and Fisette, P.: - ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, + ROBOTRAN: a powerful symbolic generator of multibody models, Mech. Sci., 4, 199–219, https://doi.org/10.5194/ms-4-199-2013, 2013. """ q = self.compute_q()(self.q_u, self.q_v_init) @@ -404,11 +702,68 @@ def partitioned_forward_dynamics_with_qv(self) -> Function: @cache_function def partitioned_forward_dynamics_full(self) -> Function: """ - Sources + Compute forward dynamics in reduced (partitioned) coordinates with explicit dependency handling. + + This method eliminates the dependent coordinates q_v by expressing the dynamics entirely + in terms of the independent coordinates q_u. The result is a reduced-order system that + automatically satisfies the holonomic constraints. + + Mathematical Formulation + ------------------------ + Starting from the full constrained dynamics: + M(q)q̈ + h(q,q̇) = τ + Jᵀλ + Φ̈ = J(q)q̈ + J̇q̇ = 0 + + Partition coordinates as q = [q_u; q_v] and the mass matrix: + M = [M_uu M_uv] + [M_vu M_vv] + + Using the constraint relationships: + q̇_v = B_vu q̇_u + q̈_v = B_vu q̈_u + b_v + + The reduced dynamics becomes: + M̄ q̈_u = τ̄ - h̄ + + where the modified (reduced) mass matrix is: + M̄ = M_uu + M_uv B_vu + B_vu^T M_vu + B_vu^T M_vv B_vu + + and the modified forces account for the dependent coordinates: + τ̄ = τ_u + B_vu^T τ_v + h̄ = h_u + B_vu^T h_v + (M_uv + B_vu^T M_vv) b_v + + Advantages + ---------- + - Smaller system to solve (n_u DOF instead of n) + - No constraint drift (constraints satisfied by construction) + - More efficient than full augmented formulation + - O(n_u³) instead of O(n³) complexity + + Returns ------- - Docquier, N., Poncelet, A., and Fisette, P.: - ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, - https://doi.org/10.5194/ms-4-199-2013, 2013. + Function + CasADi Function with signature: + Inputs: q, qdot_u, tau + Output: qddot_u (independent coordinate accelerations) + + Notes + ----- + - Requires q (full coordinates) and qdot_u (independent velocities only) + - Dependent velocities are computed internally using coupling_matrix + - The method assumes J_v is invertible (verified during setup) + + See Also + -------- + partitioned_forward_dynamics : Wrapper that also computes q from q_u + coupling_matrix : Computes B_vu = -J_v⁻¹ J_u + bias_vector : Computes b_v = -J_v⁻¹(J̇q̇) + constrained_forward_dynamics : Full-coordinate formulation with Lagrange multipliers + + References + ---------- + .. [1] Docquier, N., Poncelet, A., and Fisette, P. (2013). + ROBOTRAN: a powerful symbolic generator of multibody models. + Mech. Sci., 4, 199–219. https://doi.org/10.5194/ms-4-199-2013 """ # compute q and qdot @@ -446,7 +801,7 @@ def partitioned_forward_dynamics_full(self) -> Function: modified_generalized_forces = tau_u + coupling_matrix_vu.T @ tau_v qddot_u = inv(modified_mass_matrix) @ ( - modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect + modified_generalized_forces - second_term @ self.bias_vector(q, qdot) - modified_non_linear_effect ) casadi_fun = Function( @@ -461,13 +816,54 @@ def partitioned_forward_dynamics_full(self) -> Function: def coupling_matrix(self, q: MX) -> MX: """ - Also denoted as Bvu in the literature. + Compute the coupling matrix B_vu relating independent and dependent velocity coordinates. - Sources + The coupling matrix (also denoted as B_vu in the literature) relates the velocities of + dependent coordinates q̇_v to independent coordinates q̇_u through the constraint equation: + q̇_v = B_vu · q̇_u + + Mathematical Derivation + ----------------------- + From the velocity-level constraint equation: + Φ̇ = J(q)q̇ = 0 + + Partitioning the Jacobian J = [J_u | J_v] and coordinates q̇ = [q̇_u; q̇_v]: + J_u q̇_u + J_v q̇_v = 0 + + Solving for q̇_v (assuming J_v is invertible): + q̇_v = -J_v⁻¹ J_u q̇_u = B_vu q̇_u + + where B_vu = -J_v⁻¹ J_u. + + Parameters + ---------- + q : MX + Generalized coordinates, shape (n × 1). + + Returns ------- - Docquier, N., Poncelet, A., and Fisette, P.: - ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, - https://doi.org/10.5194/ms-4-199-2013, 2013. + MX + Coupling matrix B_vu, shape (n_v × n_u) where: + - n_v is the number of dependent coordinates + - n_u is the number of independent coordinates + + Notes + ----- + - The matrix J_v must be invertible for the coupling matrix to be well-defined + - This is verified during setup by check_dependant_jacobian() + - The coupling matrix is used extensively in the partitioned formulation + + See Also + -------- + compute_qdot_v : Uses this matrix to compute dependent velocities + bias_vector : Acceleration-level equivalent + partitioned_forward_dynamics_full : Uses coupling matrix in reduced dynamics + + References + ---------- + .. [1] Docquier, N., Poncelet, A., and Fisette, P. (2013). + ROBOTRAN: a powerful symbolic generator of multibody models. + Mech. Sci., 4, 199–219. https://doi.org/10.5194/ms-4-199-2013 """ partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(q) partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :] @@ -477,28 +873,72 @@ def coupling_matrix(self, q: MX) -> MX: return -partitioned_constraints_jacobian_v_inv @ partitioned_constraints_jacobian_u - def biais_vector(self, q: MX, qdot: MX) -> MX: + def bias_vector(self, q: MX, qdot: MX) -> MX: """ - Sources + Compute the partitioned bias vector for dependent coordinate accelerations. + + This method computes the velocity-dependent acceleration term that appears in the + relationship between dependent and independent accelerations: + q̈_v = B_vu · q̈_u + b_v + + where b_v is the bias vector computed by this method. + + Mathematical Derivation + ----------------------- + Taking the time derivative of the velocity constraint: + d/dt(J_u q̇_u + J_v q̇_v) = 0 + J_u q̈_u + J̇_u q̇_u + J_v q̈_v + J̇_v q̇_v = 0 + + Rearranging: + q̈_v = -J_v⁻¹(J_u q̈_u + J̇_u q̇_u + J̇_v q̇_v) + = B_vu q̈_u - J_v⁻¹(J̇_u q̇_u + J̇_v q̇_v) + + The bias vector is: + b_v = -J_v⁻¹ · (J̇q̇) + + where J̇q̇ is computed using the Hessian method. + + Parameters + ---------- + q : MX + Generalized coordinates, shape (n × 1). + qdot : MX + Generalized velocities, shape (n × 1). + + Returns ------- - Docquier, N., Poncelet, A., and Fisette, P.: - ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, - https://doi.org/10.5194/ms-4-199-2013, 2013. + MX + Bias vector b_v, shape (n_v × 1) where n_v is the number of dependent coordinates. - The right term of the equation (15) in the paper. + Notes + ----- + This corresponds to equation (15) in the ROBOTRAN paper. The bias vector represents + the quadratic velocity terms in the acceleration-level constraint equation. + + See Also + -------- + coupling_matrix : Velocity-level coupling matrix B_vu + compute_qddot_v : Uses this bias vector to compute dependent accelerations + holonomic_constraints_bias : Full-coordinate version of the bias term + + References + ---------- + .. [1] Docquier, N., Poncelet, A., and Fisette, P. (2013). + ROBOTRAN: a powerful symbolic generator of multibody models. + Mech. Sci., 4, 199–219. https://doi.org/10.5194/ms-4-199-2013 """ partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(q) partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :] partitioned_constraints_jacobian_v_inv = inv(partitioned_constraints_jacobian_v) - return -partitioned_constraints_jacobian_v_inv @ self.holonomic_constraints_jacobian(qdot) @ qdot + return -partitioned_constraints_jacobian_v_inv @ self.holonomic_constraints_bias(q, qdot) def state_from_partition(self, state_u: MX, state_v: MX) -> MX: """ Sources ------- Docquier, N., Poncelet, A., and Fisette, P.: - ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, + ROBOTRAN: a powerful symbolic generator of multibody models, Mech. Sci., 4, 199–219, https://doi.org/10.5194/ms-4-199-2013, 2013. """ self.check_state_u_size(state_u) @@ -526,7 +966,44 @@ def check_state_v_size(self, state_v): @cache_function def compute_q_v(self) -> Function: """ - Compute the dependent joint positions (q_v) from the independent joint positions (q_u). + Compute dependent coordinate positions from independent coordinate positions. + + This function solves the nonlinear holonomic constraint equation: + Φ(q_u, q_v) = 0 + + for the dependent coordinates q_v, given the independent coordinates q_u. + + Solution Method + --------------- + Uses Newton's method (via CasADi's rootfinder) to solve the implicit equation. + The algorithm iteratively refines q_v starting from an initial guess q_v_init until + the constraint residual ||Φ|| < tolerance. + + Parameters (via CasADi Function) + --------------------------------- + q_u : MX + Independent coordinate positions, shape (n_u × 1). + q_v_init : MX + Initial guess for dependent coordinates, shape (n_v × 1). + Use zeros if no better guess is available. + + Returns (via CasADi Function) + ------------------------------ + q_v : MX + Dependent coordinate positions satisfying Φ(q_u, q_v) = 0, shape (n_v × 1). + + Notes + ----- + - Convergence depends on the quality of q_v_init + - For time-stepping simulations, use the previous time step's q_v as initial guess + - Tolerance is controlled by set_newton_tol() (default: 1e-10) + - This is a nonlinear solve, unlike the linear relationships for velocities/accelerations + + See Also + -------- + compute_q : Wrapper that reconstructs full coordinates q from q_u + compute_qdot_v : Velocity-level equivalent (linear) + set_newton_tol : Adjust convergence tolerance """ q_v_sym = MX.sym("q_v_sym", self.nb_dependent_joints) q_u_sym = MX.sym("q_u_sym", self.q_u.shape[0], self.q_u.shape[1]) @@ -551,15 +1028,331 @@ def compute_q_v(self) -> Function: @cache_function def compute_q(self) -> Function: """ - If you don't know what to put as a q_v_init, use zeros. + Reconstruct full generalized coordinates from independent coordinates. + + This function computes the complete set of generalized coordinates q by: + 1. Solving for dependent coordinates: q_v = solve(Φ(q_u, q_v) = 0) + 2. Reassembling: q = [q_u; q_v] in the original coordinate ordering + + Parameters (via CasADi Function) + --------------------------------- + q_u : MX + Independent coordinate positions, shape (n_u × 1). + q_v_init : MX + Initial guess for dependent coordinates, shape (n_v × 1). + Use zeros if no better initial guess is available. + + Returns (via CasADi Function) + ------------------------------ + q : MX + Full generalized coordinates satisfying Φ(q) = 0, shape (n × 1). + + Notes + ----- + - The output q respects the original joint ordering in the model + - Internally uses compute_q_v() to solve for dependent coordinates + - For better convergence in simulations, warm-start q_v_init with previous values + + See Also + -------- + compute_q_v : Solves for dependent coordinates only + compute_qdot : Velocity-level equivalent + state_from_partition : Low-level function for reassembling coordinates """ q_v = self.compute_q_v()(self.q_u, self.q_v_init) biorbd_return = self.state_from_partition(self.q_u, q_v) casadi_fun = Function("compute_q", [self.q_u, self.q_v_init], [biorbd_return], ["q_u", "q_v_init"], ["q"]) return casadi_fun + def compute_q_from_u_iterative(self, q_u_array: np.ndarray, q_v_init: np.ndarray = None) -> np.ndarray: + """ + Reconstruct full coordinate trajectories from independent coordinate trajectories. + + This method is useful for post-processing optimal control solutions that only contain + independent coordinates (q_u), reconstructing the complete state trajectory including + dependent coordinates (q_v). + + The function iterates through each time point, solving for q_v at each step and using + the previous solution as a warm start for the next, which improves convergence and + computational efficiency. + + Parameters + ---------- + q_u_array : np.ndarray + Independent coordinate trajectory, shape (n_u × n_nodes) where: + - n_u is the number of independent coordinates + - n_nodes is the number of time points + q_v_init : np.ndarray, optional + Initial guess for dependent coordinates at the first node, shape (n_v,) or (n_v × 1). + If None, uses zeros. For subsequent nodes, the solution from the previous node + is used as the initial guess. + + Returns + ------- + np.ndarray + Full coordinate trajectory, shape (n × n_nodes) where n = n_u + n_v. + Coordinates are arranged in the original model ordering. + + Examples + -------- + Reconstruct full trajectory from optimal control solution: + + >>> from bioptim import SolutionMerge + >>> # Assuming 'sol' is the solution from ocp.solve() + >>> states = sol.decision_states(to_merge=SolutionMerge.NODES) + >>> q_u_traj = states["q_u"] # shape (n_u, n_nodes) + >>> + >>> # Reconstruct full coordinates + >>> q_full = model.compute_q_from_u_iterative(q_u_traj) + >>> # q_full has shape (model.nb_q, n_nodes) + + Notes + ----- + - The method uses warm-starting: q_v from node i initializes the solve at node i+1 + - This significantly improves convergence compared to using zeros at each node + - For the first node, either provide q_v_init or zeros will be used + - Convergence tolerance is controlled by the model's Newton tolerance (set_newton_tol) + + See Also + -------- + compute_q : Single-point version (CasADi Function) + compute_q_v : Computes only dependent coordinates + set_newton_tol : Adjust convergence tolerance for constraint solving + + Raises + ------ + ValueError + If q_u_array has incorrect first dimension (not matching nb_independent_joints) + """ + # Validate input shape + if q_u_array.shape[0] != self.nb_independent_joints: + raise ValueError( + f"First dimension of q_u_array must match number of independent joints. " + f"Expected {self.nb_independent_joints}, got {q_u_array.shape[0]}" + ) + + n_nodes = q_u_array.shape[1] if q_u_array.ndim > 1 else 1 + + # Handle 1D input + if q_u_array.ndim == 1: + q_u_array = q_u_array[:, np.newaxis] + + # Initialize output and warm-start vector + q_full = np.zeros((self.nb_q, n_nodes)) + + if q_v_init is None: + q_v_init = DM.zeros(self.nb_dependent_joints) + else: + q_v_init = DM(q_v_init.flatten()) + + # Iterate through time nodes + for i in range(n_nodes): + q_u_i = q_u_array[:, i] + + # Solve for dependent coordinates using previous solution as warm start + q_v_i = self.compute_q_v()(q_u_i, q_v_init).toarray().flatten() + + # Reconstruct full coordinate vector + q_full[:, i] = self.state_from_partition(q_u_i[:, np.newaxis], q_v_i[:, np.newaxis]).toarray().flatten() + + # Warm-start next iteration with current solution + q_v_init = DM(q_v_i) + + return q_full + + def compute_all_states_from_u_iterative( + self, q_u_array: np.ndarray, qdot_u_array: np.ndarray, tau_array: np.ndarray, q_v_init: np.ndarray = None + ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """ + Reconstruct all state trajectories from independent coordinates and controls. + + This method computes the complete state trajectory (positions, velocities, accelerations, + and Lagrange multipliers) from the independent coordinate trajectories and control inputs. + It is designed for post-processing optimal control solutions. + + The method iterates through each time point, solving for dependent coordinates and computing + all derived quantities using the partitioned dynamics formulation. Warm-starting is used to + improve convergence. + + Parameters + ---------- + q_u_array : np.ndarray + Independent coordinate trajectory, shape (n_u × n_nodes). + qdot_u_array : np.ndarray + Independent velocity trajectory, shape (n_u × n_nodes). + tau_array : np.ndarray + Control torque trajectory for all joints, shape (nb_tau × n_controls). + Typically n_controls = n_nodes - 1 for piecewise constant controls. + q_v_init : np.ndarray, optional + Initial guess for dependent coordinates at the first node, shape (n_v,). + If None, uses zeros. Subsequent nodes use warm-starting from previous solutions. + + Returns + ------- + q : np.ndarray + Full coordinate trajectory, shape (nb_q × n_nodes). + qdot : np.ndarray + Full velocity trajectory, shape (nb_q × n_nodes). + qddot : np.ndarray + Full acceleration trajectory, shape (nb_q × n_nodes). + lambdas : np.ndarray + Lagrange multiplier trajectory, shape (n_v × n_nodes). + Represents constraint forces in the dependent directions. + + Examples + -------- + Compute all states from optimal control solution: + + >>> from bioptim import SolutionMerge + >>> states = sol.decision_states(to_merge=SolutionMerge.NODES) + >>> controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + >>> + >>> # Prepare tau array (pad with zeros for final node if needed) + >>> n_nodes = states["q_u"].shape[1] + >>> tau = np.zeros((model.nb_tau, n_nodes)) + >>> tau[:, :-1] = controls["tau"] + >>> + >>> # Compute all states + >>> q, qdot, qddot, lambdas = model.compute_all_states_from_u_iterative( + ... states["q_u"], + ... states["qdot_u"], + ... tau + ... ) + + Notes + ----- + - Uses warm-starting: each node initializes from the previous solution + - Lagrange multipliers represent constraint forces maintaining the holonomic constraints + - The tau_array should contain torques for all joints (both independent and dependent) + - If tau has fewer columns than state nodes, the last column is assumed to be zero + + See Also + -------- + compute_q_from_u_iterative : Compute only positions + compute_the_lagrangian_multipliers : Single-point Lagrange multiplier computation + partitioned_forward_dynamics : Forward dynamics in reduced coordinates + + Raises + ------ + ValueError + If array shapes are incompatible with the model dimensions. + """ + # Validate input shapes + if q_u_array.shape[0] != self.nb_independent_joints: + raise ValueError( + f"First dimension of q_u_array must match number of independent joints. " + f"Expected {self.nb_independent_joints}, got {q_u_array.shape[0]}" + ) + if qdot_u_array.shape[0] != self.nb_independent_joints: + raise ValueError( + f"First dimension of qdot_u_array must match number of independent joints. " + f"Expected {self.nb_independent_joints}, got {qdot_u_array.shape[0]}" + ) + if tau_array.shape[0] != self.nb_tau: + raise ValueError( + f"First dimension of tau_array must match number of torques. " + f"Expected {self.nb_tau}, got {tau_array.shape[0]}" + ) + + n_nodes = q_u_array.shape[1] if q_u_array.ndim > 1 else 1 + n_controls = tau_array.shape[1] if tau_array.ndim > 1 else 1 + + # Handle 1D inputs + if q_u_array.ndim == 1: + q_u_array = q_u_array[:, np.newaxis] + if qdot_u_array.ndim == 1: + qdot_u_array = qdot_u_array[:, np.newaxis] + if tau_array.ndim == 1: + tau_array = tau_array[:, np.newaxis] + + # Pad tau with zeros if needed (for final node in piecewise constant control) + if n_controls < n_nodes: + tau_padded = np.zeros((self.nb_tau, n_nodes)) + tau_padded[:, :n_controls] = tau_array + tau_array = tau_padded + + # Initialize outputs + q = np.zeros((self.nb_q, n_nodes)) + qdot = np.zeros((self.nb_q, n_nodes)) + qddot = np.zeros((self.nb_q, n_nodes)) + lambdas = np.zeros((self.nb_dependent_joints, n_nodes)) + + # Initialize warm-start + if q_v_init is None: + q_v_init = DM.zeros(self.nb_dependent_joints) + else: + q_v_init = DM(q_v_init.flatten()) + + # Iterate through time nodes + for i in range(n_nodes): + q_u_i = q_u_array[:, i] + qdot_u_i = qdot_u_array[:, i] + tau_i = tau_array[:, i] + + # Solve for dependent coordinates + q_v_i = self.compute_q_v()(q_u_i, q_v_init).toarray().flatten() + + # Reconstruct full state + q[:, i] = self.state_from_partition(q_u_i[:, np.newaxis], q_v_i[:, np.newaxis]).toarray().flatten() + + # Compute full velocity + qdot[:, i] = self.compute_qdot()(q[:, i], qdot_u_i).toarray().flatten() + + # Compute independent accelerations from forward dynamics + qddot_u_i = self.partitioned_forward_dynamics()(q_u_i, qdot_u_i, q_v_init, tau_i).toarray().flatten() + + # Compute full acceleration + qddot[:, i] = self.compute_qddot()(q[:, i], qdot[:, i], qddot_u_i).toarray().flatten() + + # Compute Lagrange multipliers (constraint forces) + lambdas[:, i] = ( + self.compute_the_lagrangian_multipliers()(q_u_i[:, np.newaxis], qdot_u_i, q_v_init, tau_i) + .toarray() + .flatten() + ) + + # Warm-start next iteration + q_v_init = DM(q_v_i) + + return q, qdot, qddot, lambdas + @cache_function def compute_qdot_v(self) -> Function: + """ + Compute dependent coordinate velocities from independent coordinate velocities. + + This function computes the velocities of dependent coordinates q̇_v given the + velocities of independent coordinates q̇_u, using the constraint relationship: + q̇_v = B_vu · q̇_u + + where B_vu = -J_v⁻¹ J_u is the coupling matrix. + + This relationship is derived from the velocity-level constraint: + Φ̇ = J_u q̇_u + J_v q̇_v = 0 + + Parameters (via CasADi Function) + --------------------------------- + q : MX + Full generalized coordinates, shape (n × 1). + qdot_u : MX + Independent coordinate velocities, shape (n_u × 1). + + Returns (via CasADi Function) + ------------------------------ + qdot_v : MX + Dependent coordinate velocities, shape (n_v × 1). + + Notes + ----- + This is the fundamental velocity relationship in partitioned coordinate formulations. + It ensures that the velocity-level constraint Φ̇ = 0 is satisfied. + + See Also + -------- + coupling_matrix : Computes the B_vu matrix used in this relationship + compute_qddot_v : Acceleration-level equivalent + compute_qdot : Reconstructs full velocity vector from q̇_u and q̇_v + """ coupling_matrix_vu = self.coupling_matrix(self.q) biorbd_return = coupling_matrix_vu @ self.qdot_u casadi_fun = Function("compute_qdot_v", [self.q, self.qdot_u], [biorbd_return], ["q", "qdot_u"], ["qdot_v"]) @@ -588,15 +1381,52 @@ def compute_qdot(self) -> Function: @cache_function def compute_qddot_v(self) -> Function: """ - Sources - ------- - Docquier, N., Poncelet, A., and Fisette, P.: - ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, - https://doi.org/10.5194/ms-4-199-2013, 2013. - Equation (17) in the paper. + Compute dependent coordinate accelerations from independent coordinate accelerations. + + This function computes the accelerations of dependent coordinates q̈_v given the + accelerations of independent coordinates q̈_u, using the constraint relationship: + q̈_v = B_vu · q̈_u + b_v + + where: + - B_vu = -J_v⁻¹ J_u is the coupling matrix + - b_v = -J_v⁻¹(J̇q̇) is the bias vector + + This relationship ensures the acceleration-level constraint Φ̈ = 0 is satisfied. + + Parameters (via CasADi Function) + --------------------------------- + q : MX + Full generalized coordinates, shape (n × 1). + qdot : MX + Full generalized velocities, shape (n × 1). + qddot_u : MX + Independent coordinate accelerations, shape (n_u × 1). + + Returns (via CasADi Function) + ------------------------------ + qddot_v : MX + Dependent coordinate accelerations, shape (n_v × 1). + + Notes + ----- + Corresponds to equation (17) in the ROBOTRAN paper. This is the acceleration-level + equivalent of the velocity relationship q̇_v = B_vu q̇_u. + + See Also + -------- + coupling_matrix : Computes the B_vu matrix + bias_vector : Computes the b_v bias term + compute_qdot_v : Velocity-level equivalent + compute_qddot : Reconstructs full acceleration vector from q̈_u and q̈_v + + References + ---------- + .. [1] Docquier, N., Poncelet, A., and Fisette, P. (2013). + ROBOTRAN: a powerful symbolic generator of multibody models. + Mech. Sci., 4, 199–219. https://doi.org/10.5194/ms-4-199-2013 """ coupling_matrix_vu = self.coupling_matrix(self.q) - biorbd_return = coupling_matrix_vu @ self.qddot_u + self.biais_vector(self.q, self.qdot) + biorbd_return = coupling_matrix_vu @ self.qddot_u + self.bias_vector(self.q, self.qdot) casadi_fun = Function( "compute_qddot_v", [self.q, self.qdot, self.qddot_u], [biorbd_return], ["q", "qdot", "qddot_u"], ["qddot_v"] ) diff --git a/bioptim/models/protocols/holonomic_biomodel.py b/bioptim/models/protocols/holonomic_biomodel.py index d8b5bb6d5..c3b36d38f 100644 --- a/bioptim/models/protocols/holonomic_biomodel.py +++ b/bioptim/models/protocols/holonomic_biomodel.py @@ -296,9 +296,9 @@ def coupling_matrix(self, q: MX) -> MX: https://doi.org/10.5194/ms-4-199-2013, 2013. """ - def biais_vector(self, q: MX, qdot: MX) -> MX: + def bias_vector(self, q: MX, qdot: MX) -> MX: """ - Compute the biais vector, denoted b in the paper : + Compute the bias vector, denoted b in the paper : Sources ------- diff --git a/bioptim/models/protocols/holonomic_constraints.py b/bioptim/models/protocols/holonomic_constraints.py index 13ad25bbd..7195a0d11 100644 --- a/bioptim/models/protocols/holonomic_constraints.py +++ b/bioptim/models/protocols/holonomic_constraints.py @@ -1,10 +1,10 @@ """ -This class contains different holonomic constraint function. +This module contains holonomic constraint functions for biomechanical models. """ from typing import Any, Callable -from casadi import MX, Function, jacobian, vertcat +from casadi import MX, Function, jacobian, vertcat, trace, sqrt, DM from .biomodel import BioModel from ...misc.options import OptionDict @@ -12,7 +12,24 @@ class HolonomicConstraintsFcn: """ - This class contains different holonomic constraint. + Factory class providing static methods to generate holonomic constraint functions. + + This class contains various holonomic constraint formulations that can be applied to + biomechanical models. Each method returns a tuple of CasADi Functions representing: + - The constraint equation Φ(q) = 0 + - The constraint Jacobian ∂Φ/∂q + - The bias term J̇q̇ for acceleration-level constraints + + Available Constraints + --------------------- + - superimpose_markers: Enforce two markers to occupy the same position + - align_frames: Enforce two reference frames to maintain a specified relative orientation + - rigid_contacts: Enforce rigid contact constraints to prevent penetration + + See Also + -------- + HolonomicConstraintsList : Container for managing multiple holonomic constraints + HolonomicBiorbdModel : Model class that uses these constraints """ @staticmethod @@ -24,26 +41,66 @@ def superimpose_markers( model: BioModel = None, ) -> tuple[Function, Function, Function]: """ - Generate the constraint functions to superimpose two markers. + Generate holonomic constraint functions to superimpose two markers or fix a marker to the origin. + + This constraint enforces that two markers occupy the same position in space (or a marker + is fixed to the origin), creating a holonomic constraint of the form: + Φ(q) = marker_1(q) - marker_2(q) = 0 + + The constraint can be expressed in either the global frame or a local reference frame, + and can be applied to specific spatial dimensions via the index parameter. Parameters ---------- - model: BioModel - The model. - marker_1: str - The name of the first marker. - marker_2: str - The name of the second marker. If None, the constraint will be to superimpose the first marker to the - origin. - index: slice - The index of the markers to superimpose. - local_frame_index: int - The index of the frame in which the markers are expressed. If None, the markers are expressed in the global. - + model : BioModel + The biomechanical model containing the marker definitions. + marker_1 : str + Name of the first marker in the model. + marker_2 : str, optional + Name of the second marker. If None, marker_1 is constrained to the origin [0, 0, 0]. + index : slice, default=slice(0, 3) + Slice object specifying which spatial dimensions to constrain. + Examples: + - slice(0, 3): all three dimensions (x, y, z) + - slice(0, 2): only x and y dimensions + - slice(2, 3): only z dimension + local_frame_index : int, optional + Index of the segment/frame in which to express the constraint. + If None, markers are expressed in the global (world) frame. + If specified, both markers are transformed into this local frame before computing the constraint. Returns ------- - The constraint function, its jacobian and its double derivative. + tuple[Function, Function, Function] + A tuple containing three CasADi Functions: + - constraints_func: Φ(q) → constraint values (m × 1) + - constraints_jacobian_func: ∂Φ/∂q → constraint Jacobian (m × n) + - bias_func: (q, q̇) → J̇q̇ → bias/acceleration term (m × 1) + + where m is the number of constrained dimensions (determined by index), + and n is the number of generalized coordinates. + + Examples + -------- + Superimpose two markers in all three dimensions: + >>> constraint = HolonomicConstraintsFcn.superimpose_markers( + ... model=my_model, + ... marker_1="hand", + ... marker_2="target" + ... ) + + Fix a marker to the origin in x-y plane only: + >>> constraint = HolonomicConstraintsFcn.superimpose_markers( + ... model=my_model, + ... marker_1="foot", + ... marker_2=None, + ... index=slice(0, 2) + ... ) + + See Also + -------- + align_frames : Constraint to align orientations of two reference frames + compute_bias_vector : Computes the bias term J̇q̇ """ # symbolic variables to create the functions @@ -89,22 +146,208 @@ def superimpose_markers( ["holonomic_constraints_jacobian"], ).expand() - # the double derivative of the constraint - constraints_double_derivative = ( - constraints_jacobian_func(q_sym) @ q_ddot_sym + constraints_jacobian_func(q_dot_sym) @ q_dot_sym - ) + bias_vector = compute_bias_vector(constraints_jacobian, q_sym, q_dot_sym) - constraints_double_derivative_func = Function( - "holonomic_constraints_double_derivative", - [q_sym, q_dot_sym, q_ddot_sym], - [constraints_double_derivative], - ["q", "q_dot", "q_ddot"], - ["holonomic_constraints_double_derivative"], + bias_func = Function( + "superimpose_markers_bias", + [q_sym, q_dot_sym], + [bias_vector], + ["q", "q_dot"], + ["superimpose_markers_bias"], ).expand() - return constraints_func, constraints_jacobian_func, constraints_double_derivative_func + return constraints_func, constraints_jacobian_func, bias_func @staticmethod + def align_frames( + model: BioModel = None, + frame_1_idx: int = 0, + frame_2_idx: int = 1, + local_frame_idx: int = None, + relative_rotation=DM.eye(3), + ) -> tuple[Function, Function, Function]: + """ + Generate holonomic constraint functions to align the orientation of two reference frames. + + This constraint enforces that two body-fixed frames maintain a specified relative orientation, + creating a 3-DOF holonomic constraint on the system's configuration. + + Mathematical Formulation + ------------------------ + The constraint enforces: + R₁ᵀ R₂ = R_desired + + where R₁ and R₂ are the rotation matrices of frames 1 and 2, and R_desired is the + desired relative rotation (identity by default). + + The constraint is formulated using the axis-angle representation of the rotation error. + For a relative rotation R_rel = R_desired^T (R₁ᵀ R₂), we extract the three independent + components of the skew-symmetric part: + + S = (R_rel - R_rel^T) / 2 + + When the frames are aligned (θ = 0), S = 0. The constraint vector consists of the + three independent components of S: + + Φ(q) = [S₃₂, -S₃₁, S₂₁]^T = 0 + + This formulation: + - Uses exactly 3 scalar equations (minimal representation) + - Avoids singularities at θ = 0 through Taylor expansion + - Is equivalent to the axis-angle formulation: Φ = (θ/sin(θ)) · ω̂ + + Parameters + ---------- + model : BioModel + The biomechanical model containing the kinematic tree. + frame_1_idx : int, default=0 + Index of the first segment/frame in the model. + frame_2_idx : int, default=1 + Index of the second segment/frame whose orientation will be aligned with frame_1. + local_frame_idx : int, optional + If specified, both frames are first transformed into this local reference frame + before computing the relative rotation. When None, the global (world) frame is used. + This is useful for expressing alignment constraints relative to a moving reference. + relative_rotation : DM, default=DM.eye(3) + The desired 3×3 relative rotation matrix between the frames. + Default is identity, meaning frames should be perfectly aligned. + For non-identity values, the constraint enforces R₁ᵀ R₂ = relative_rotation. + + Returns + ------- + tuple[Function, Function, Function] + A tuple containing three CasADi Functions: + - constraints_func: Φ(q) → constraint values (3 × 1) + - constraints_jacobian_func: ∂Φ/∂q → constraint Jacobian (3 × n) + - bias_func: (q, q̇) → J̇q̇ → bias/acceleration term (3 × 1) + + where n is the number of generalized coordinates. + + Examples + -------- + Align two segments in the global frame: + >>> constraint = HolonomicConstraintsFcn.align_frames( + ... model=my_model, + ... frame_1_idx=2, # pelvis + ... frame_2_idx=5 # torso + ... ) + + Align with a 90-degree rotation about z-axis: + >>> import numpy as np + >>> R_z_90 = DM([ + ... [0, -1, 0], + ... [1, 0, 0], + ... [0, 0, 1] + ... ]) + >>> constraint = HolonomicConstraintsFcn.align_frames( + ... model=my_model, + ... frame_1_idx=2, + ... frame_2_idx=5, + ... relative_rotation=R_z_90 + ... ) + + Notes + ----- + The Taylor expansion used for θ/sin(θ) provides numerical stability near θ = 0: + θ/sin(θ) ≈ 1 + θ²/6 + 7θ⁴/360 + 31θ⁶/15120 + + This ensures the constraint remains well-conditioned even when the frames are + nearly aligned. + + See Also + -------- + superimpose_markers : Constraint to superimpose marker positions + compute_bias_vector : Computes the bias term J̇q̇ + + References + ---------- + .. [1] Murray, R. M., Li, Z., & Sastry, S. S. (1994). A Mathematical Introduction + to Robotic Manipulation. CRC Press. + """ + # Symbolic variables + q_sym = MX.sym("q", model.nb_q, 1) # generalized coordinates + q_dot_sym = MX.sym("q_dot", model.nb_qdot, 1) # velocities + q_ddot_sym = MX.sym("q_ddot", model.nb_qdot, 1) # accelerations + parameters = model.parameters # optional model parameters + + # Homogeneous transformation matrices of the two frames + # Global homogeneous matrices (4×4) of the two frames + # T1_glob = model.homogeneous_matrices_in_global(segment_index=frame_1_idx)(q_sym, parameters) # shape (4,4) + # T2_glob = model.homogeneous_matrices_in_global(segment_index=frame_2_idx)(q_sym, parameters) # shape (4,4) + + # If a *local* reference frame is requested we first bring the two frames + # into that local frame (identical to the logic used for the marker + # constraint). + if local_frame_idx is not None: + # Get the rotation matrix of the local frame in the global frame + R_loc_glob = model.homogeneous_matrices_in_global(segment_index=local_frame_idx)(q_sym, parameters)[:3, :3] + + # Get the rotation matrices of the two frames in the global frame + R1_glob = model.homogeneous_matrices_in_global(segment_index=frame_1_idx)(q_sym, parameters)[:3, :3] + R2_glob = model.homogeneous_matrices_in_global(segment_index=frame_2_idx)(q_sym, parameters)[:3, :3] + + # Transform the rotation matrices into the local frame + R1 = R_loc_glob.T @ R1_glob + R2 = R_loc_glob.T @ R2_glob + else: + # If no local frame is specified, use the global frame + R1 = model.homogeneous_matrices_in_global(segment_index=frame_1_idx)(q_sym, parameters)[:3, :3] + R2 = model.homogeneous_matrices_in_global(segment_index=frame_2_idx)(q_sym, parameters)[:3, :3] + + # Relative rotation: R_rel = R1ᵀ·R2 (frame‑1 → frame‑2) + R_rel = R1.T @ R2 # still a symbolic 3×3 matrix + + # Error in relative rotation: R_rel - R_desired + # R_error = R_rel @ relative_rotation.T + R_error = relative_rotation.T @ R_rel + + # Minimal set of scalar constraints (3 equations) + # The skew‑symmetric part of a proper rotation is zero when the angle is zero: + # S = (R_rel - R_relᵀ) / 2 → S = 0 ⇔ ω = 0, θ = 0 + # We vectorise the three independent components of S: + # S_21, S_31, S_32 + # (any consistent ordering works, we keep the same order used in the + # analytical derivation of the constraint in the OP.) + cos_theta = (trace(R_error) - 1) / 2 + theta = sqrt(2 * (1 - cos_theta) + 1e-12) # using the first-order expansion of arccos + theta_over_sintheta = ( + 1 + theta**2 / 6 + 7 * theta**4 / 360 + 31 * theta**6 / 15120 + ) # using the Taylor expansion + S = theta_over_sintheta * (R_error - R_error.T) / 2.0 # still 3×3, skew‑symmetric + + constraint = vertcat(S[2, 1], -S[2, 0], S[1, 0]) # r32 - r23 # r13 - r31 # r21 - r12 + + # Jacobian and second derivative (CasADi) + constraints_jacobian = jacobian(constraint, q_sym) + + constraints_func = Function( + "align_frames_constraint", + [q_sym], + [constraint], + ["q"], + ["c_align"], + ).expand() + + constraints_jacobian_func = Function( + "align_frames_jacobian", + [q_sym], + [constraints_jacobian], + ["q"], + ["J_align"], + ).expand() + + bias_vector = compute_bias_vector(constraints_jacobian, q_sym, q_dot_sym) + + bias_func = Function( + "bias_align_frames", + [q_sym, q_dot_sym], + [bias_vector], + ["q", "q_dot"], + ["bias_align_frames"], + ).expand() + + return constraints_func, constraints_jacobian_func, bias_func + def rigid_contacts( model: BioModel = None, ) -> tuple[Function, Function, Function]: @@ -124,7 +367,6 @@ def rigid_contacts( # symbolic variables to create the functions q_sym = MX.sym("q", model.nb_q, 1) q_dot_sym = MX.sym("q_dot", model.nb_qdot, 1) - q_ddot_sym = MX.sym("q_ddot", model.nb_qdot, 1) parameters = model.parameters contact_position = MX() @@ -139,14 +381,6 @@ def rigid_contacts( # the jacobian of the constraint constraints_jacobian = jacobian(constraint, q_sym) - # First derivative (velocity) - velocity_constraint = jacobian(constraint, q_sym) @ q_dot_sym - - # Second derivative (acceleration) - acceleration_constraint = ( - jacobian(velocity_constraint, q_sym) @ q_dot_sym + jacobian(constraint, q_sym) @ q_ddot_sym - ) - constraints_func = Function( "holonomic_constraints", [q_sym, parameters], @@ -163,25 +397,46 @@ def rigid_contacts( ["holonomic_constraints_jacobian"], ).expand() - constraints_double_derivative_func = Function( - "holonomic_constraints_double_derivative", - [q_sym, q_dot_sym, q_ddot_sym, parameters], - [acceleration_constraint], - ["q", "q_dot", "q_ddot", "parameters"], - ["holonomic_constraints_double_derivative"], + bias_vector = compute_bias_vector(constraints_jacobian, q_sym, q_dot_sym) + + bias_func = Function( + "rigid_contacts_bias", + [q_sym, q_dot_sym, parameters], + [bias_vector], + ["q", "q_dot", "parameters"], + ["rigid_contacts_bias"], ).expand() - return constraints_func, constraints_jacobian_func, constraints_double_derivative_func + return constraints_func, constraints_jacobian_func, bias_func class HolonomicConstraintsList(OptionDict): """ - A list of holonomic constraints to be sent to HolonomicBiorbdModel.set_holonomic_configuration() + Container for managing multiple holonomic constraints. + + This class stores a collection of holonomic constraints that can be applied to a + HolonomicBiorbdModel. Each constraint is identified by a unique key and includes + the constraint function along with any additional parameters. Methods ------- - add(self, key: str, constraints: Function, constraints_jacobian: Function, constraints_double_derivative: Function) - Add a new holonomic constraint to the dict + add(key, constraints_fcn, **extra_arguments) + Add a new holonomic constraint to the collection. + + Examples + -------- + >>> constraints = HolonomicConstraintsList() + >>> constraints.add( + ... "marker_constraint", + ... HolonomicConstraintsFcn.superimpose_markers, + ... marker_1="hand", + ... marker_2="target" + ... ) + + See Also + -------- + HolonomicConstraintsFcn : Factory class for creating constraint functions + HolonomicBiorbdModel.set_holonomic_configuration : Method that uses this list """ def __init__(self): @@ -203,3 +458,70 @@ def add(self, key: str, constraints_fcn: Callable, **extra_arguments: Any): constraints_fcn=constraints_fcn, extra_arguments=extra_arguments, ) + + +def compute_bias_vector(constraints_jacobian: MX, q_sym: MX, q_dot_sym: MX) -> MX: + """ + Compute the bias vector of the holonomic constraint acceleration equation using the Hessian method. + + For holonomic constraints Φ(q) = 0, the acceleration-level constraint is: + Φ̈ = J(q)q̈ + J̇(q)q̇ = 0 + + This function computes the bias term J̇(q)q̇ through the Hessian tensor of the constraint. + + Mathematical Background + ----------------------- + The time derivative of the constraint Jacobian can be computed using the chain rule: + J̇ = ∂J/∂q · q̇ = Σₖ (∂J/∂qₖ) q̇ₖ + + where ∂J/∂q is a 3rd-order tensor (Hessian) with dimensions (m × n × n): + - m: number of constraints + - n: number of generalized coordinates + + For each constraint i, the bias term is computed as a quadratic form: + (J̇q̇)ᵢ = Σⱼ Σₖ (∂Jᵢⱼ/∂qₖ) q̇ₖ q̇ⱼ = q̇ᵀ Hᵢ q̇ + + where Hᵢ is the Hessian matrix of the i-th constraint Jacobian row. + + Implementation Note + ------------------- + This method has O(n²) complexity per constraint. For systems where efficiency is critical, + spatial algebra methods (O(n)) could be considered, though the Hessian approach is more + straightforward for arbitrary holonomic constraints. + + Parameters + ---------- + constraints_jacobian : MX + The Jacobian matrix of the constraints, shape (m × n), where: + J = ∂Φ/∂q + q_sym : MX + Symbolic variable for generalized coordinates, shape (n × 1). + Required to compute the Hessian ∂J/∂q through symbolic differentiation. + q_dot_sym : MX + Symbolic variable for generalized velocities, shape (n × 1). + Used in the quadratic form q̇ᵀ H q̇. + + Returns + ------- + MX + The bias vector J̇q̇, shape (m × 1), representing the velocity-dependent + acceleration terms in the constraint equation. + + See Also + -------- + HolonomicBiorbdModel.holonomic_constraints_bias : Uses this function to compute bias terms + HolonomicConstraintsFcn.superimpose_markers : Example constraint that generates bias functions + HolonomicConstraintsFcn.align_frames : Example constraint that generates bias functions + + References + ---------- + .. [1] Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer. + .. [2] Docquier, N., Poncelet, A., and Fisette, P. (2013). ROBOTRAN: a powerful symbolic + generator of multibody models. Mech. Sci., 4, 199–219. + """ + Jdot_qdot = [] + for i in range(constraints_jacobian.shape[0]): + hessian = jacobian(constraints_jacobian[i, :], q_sym) + Jdot_qdot.append(q_dot_sym.T @ hessian @ q_dot_sym) + Jdot_qdot = vertcat(*Jdot_qdot) + return Jdot_qdot diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 25ef9084d..253429502 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -113,11 +113,11 @@ def test_model_holonomic(): # Test partitioned_forward_dynamics_with_qv TestUtils.assert_equal( - model.partitioned_forward_dynamics_with_qv()(q_u, q_v[0], qdot_u, tau), [-3.326526], expand=False + model.partitioned_forward_dynamics_with_qv()(q_u, q_v[0], qdot_u, tau), [-5.180203], expand=False ) # Test partitioned_forward_dynamics_full - TestUtils.assert_equal(model.partitioned_forward_dynamics_full()(q, qdot_u, tau), [-23.937828], expand=False) + TestUtils.assert_equal(model.partitioned_forward_dynamics_full()(q, qdot_u, tau), [81.55801], expand=False) # Test error message for non-square Jacobian ill_model = HolonomicBiorbdModel(biorbd_model_path) @@ -145,9 +145,9 @@ def test_model_holonomic(): [[-0.5104801, 0.02982221, -0.96017029], [-0.70317549, 0.13829549, 0.2794155]], ) TestUtils.assert_equal(model.holonomic_constraints_derivative(q, q_dot), [-7.65383105, -0.44473154]) - TestUtils.assert_equal(model.holonomic_constraints_double_derivative(q, q_dot, q_ddot), [10.23374996, -11.73729905]) + TestUtils.assert_equal(model.holonomic_constraints_double_derivative(q, q_dot, q_ddot), [-49.950546, -145.794884]) TestUtils.assert_equal( - model.constrained_forward_dynamics()(q, q_dot, tau, []), [-5.18551845, -3.01921376, 25.79451813] + model.constrained_forward_dynamics()(q, q_dot, tau, []), [-159.968193, 131.282108, 49.576071] ) TestUtils.assert_equal( model.partitioned_mass_matrix(q), @@ -166,9 +166,9 @@ def test_model_holonomic(): [[-0.5104801, 0.02982221, -0.96017029], [-0.70317549, 0.13829549, 0.2794155]], ) - TestUtils.assert_equal(model.partitioned_forward_dynamics()(q_u, qdot_u, q_v, tau), -1.101808, expand=False) + TestUtils.assert_equal(model.partitioned_forward_dynamics()(q_u, qdot_u, q_v, tau), -3.706354, expand=False) TestUtils.assert_equal(model.coupling_matrix(q), [5.79509793, -0.35166415], expand=False) - TestUtils.assert_equal(model.biais_vector(q, q_dot), [27.03137348, 23.97095718], expand=False) + TestUtils.assert_equal(model.bias_vector(q, q_dot), [1058.313451, -6.679008], expand=False) TestUtils.assert_equal(model.state_from_partition(q_u, q_v), q) TestUtils.assert_equal(model.compute_q_v()(q_u, q_v), [2 * np.pi / 3, 2 * np.pi / 3], expand=False) @@ -176,8 +176,9 @@ def test_model_holonomic(): TestUtils.assert_equal(model.compute_qdot_v()(q, qdot_u), [23.18039172, -1.4066566], expand=False) TestUtils.assert_equal(model.compute_qdot()(q, qdot_u), [4.0, 23.18039172, -1.4066566], expand=False) - TestUtils.assert_equal(model.compute_qddot_v()(q, q_dot, q_ddot_u), [67.597059, 21.509308], expand=False) - TestUtils.assert_equal(model.compute_qddot()(q, q_dot, q_ddot_u), [7.0, 67.597059, 21.509308], expand=False) + qddot_v_expected = [1098.879137, -9.140657] + TestUtils.assert_equal(model.compute_qddot_v()(q, q_dot, q_ddot_u), qddot_v_expected, expand=False) + TestUtils.assert_equal(model.compute_qddot()(q, q_dot, q_ddot_u), [7.0] + qddot_v_expected, expand=False) npt.assert_almost_equal( model.compute_q_v()(DM([0.0]), DM([1.0, 1.0])).toarray().squeeze(), @@ -226,18 +227,30 @@ def test_example_two_pendulums(ode_solver): [ [ 1.54, - 1.433706, - 1.185046, - 0.891157, - 0.561607, - 0.191792, - -0.206511, - -0.614976, - -1.018383, - -1.356253, + 1.46024284, + 1.24947784, + 0.98555579, + 0.69914724, + 0.37122704, + -0.03002195, + -0.51108564, + -1.00708886, + -1.38595887, -1.54, ], - [1.54, 1.669722, 1.924726, 2.127746, 2.226937, 2.184007, 1.972105, 1.593534, 1.06751, 0.507334, 0.0], + [ + 1.54, + 1.63198614, + 1.86553028, + 2.11629728, + 2.30191613, + 2.37335671, + 2.26406118, + 1.91453347, + 1.34329718, + 0.66587232, + 0.0, + ], ], decimal=6, ) @@ -247,110 +260,110 @@ def test_example_two_pendulums(ode_solver): states["q_u"], [ [ - 1.54, - 1.53947255, - 1.52829032, - 1.4918706, - 1.44772412, - 1.43369574, - 1.41898275, - 1.35974706, - 1.27447461, - 1.20430304, - 1.18502396, - 1.16556562, - 1.09140745, - 0.99166684, - 0.91261473, - 0.8911305, - 0.86947629, - 0.78671683, - 0.67482281, - 0.585796, - 0.56157817, - 0.53715892, - 0.44378604, - 0.31793766, - 0.21860716, - 0.19175812, - 0.1647666, - 0.0623588, - -0.07337049, - -0.1784637, - -0.20655284, - -0.23470758, - -0.34109507, - -0.48052131, - -0.58686908, - -0.6150277, - -0.64319721, - -0.74951225, - -0.88766222, - -0.99132882, - -1.01844059, - -1.0452361, - -1.14223391, - -1.25827372, - -1.33704454, - -1.35629421, - -1.3747351, - -1.43601269, - -1.49800372, - -1.53258632, - -1.54, + 1.54000000e00, + 1.53960704e00, + 1.53119417e00, + 1.50384285e00, + 1.47075073e00, + 1.46024237e00, + 1.44907691e00, + 1.40171151e00, + 1.32894631e00, + 1.26675802e00, + 1.24947642e00, + 1.23197591e00, + 1.16502476e00, + 1.07516835e00, + 1.00460860e00, + 9.85554734e-01, + 9.66412104e-01, + 8.93831885e-01, + 7.96802946e-01, + 7.20030415e-01, + 6.99146346e-01, + 6.78066966e-01, + 5.97057646e-01, + 4.86014637e-01, + 3.96011393e-01, + 3.71226227e-01, + 3.46078491e-01, + 2.48270204e-01, + 1.11985901e-01, + 6.46711474e-04, + -3.00226953e-02, + -6.11063003e-02, + -1.81341284e-01, + -3.45693192e-01, + -4.75959406e-01, + -5.11086426e-01, + -5.46313411e-01, + -6.78694841e-01, + -8.49012892e-01, + -9.74688088e-01, + -1.00708982e00, + -1.03891405e00, + -1.15218687e00, + -1.28236238e00, + -1.36618443e00, + -1.38595972e00, + -1.40460611e00, + -1.46407898e00, + -1.51646647e00, + -1.53726816e00, + -1.54000000e00, ], [ - 1.54, - 1.54064515, - 1.55431106, - 1.59922772, - 1.65296364, - 1.66973502, - 1.68705873, - 1.75370598, - 1.84171006, - 1.90763103, - 1.92475754, - 1.94158775, - 2.00129161, - 2.07058481, - 2.11662288, - 2.12778938, - 2.13845817, - 2.17380886, - 2.20803169, - 2.2242531, - 2.22699357, - 2.22904554, - 2.2304308, - 2.21629085, - 2.19241948, - 2.18407247, - 2.17489014, - 2.1330352, - 2.06082354, - 1.99228721, - 1.97216828, - 1.95123087, - 1.86495558, - 1.73527252, - 1.62449462, - 1.59357668, - 1.56195524, - 1.43562583, - 1.25495762, - 1.10760291, - 1.06752078, - 1.0272699, - 0.8761086, - 0.68415659, - 0.54376452, - 0.50733629, - 0.47128493, - 0.3387245, - 0.16767637, - 0.03546255, - 0.0, + 1.54000000e00, + 1.54045587e00, + 1.55019337e00, + 1.58181191e00, + 1.61992431e00, + 1.63198694e00, + 1.64477139e00, + 1.69858129e00, + 1.77966388e00, + 1.84713061e00, + 1.86553257e00, + 1.88397346e00, + 1.95230605e00, + 2.03811167e00, + 2.10032450e00, + 2.11629903e00, + 2.13194694e00, + 2.18720875e00, + 2.25063538e00, + 2.29202647e00, + 2.30191745e00, + 2.31126855e00, + 2.34100320e00, + 2.36597396e00, + 2.37329336e00, + 2.37335756e00, + 2.37255179e00, + 2.36109776e00, + 2.32464067e00, + 2.27895230e00, + 2.26406163e00, + 2.24798307e00, + 2.17667837e00, + 2.05757889e00, + 1.94679445e00, + 1.91453356e00, + 1.88116543e00, + 1.74655257e00, + 1.55089225e00, + 1.38817903e00, + 1.34329686e00, + 1.29790110e00, + 1.12405118e00, + 8.92207951e-01, + 7.13412142e-01, + 6.65871739e-01, + 6.18449247e-01, + 4.42108988e-01, + 2.16115680e-01, + 4.53319197e-02, + 0.00000000e00, ], ], decimal=6, @@ -367,7 +380,7 @@ def test_example_two_pendulums_algebraic(): bioptim_folder = TestUtils.bioptim_folder() # --- Prepare the ocp --- # - ocp, model = two_pendulums_algebraic.prepare_ocp( + ocp = two_pendulums_algebraic.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/models/two_pendulums.bioMod", n_shooting=5, final_time=1, @@ -389,38 +402,38 @@ def test_example_two_pendulums_algebraic(): [ [ 1.54, - 1.53645293, - 1.49090493, - 1.46110856, - 1.41970745, - 1.22293077, - 1.12028136, - 1.01244361, - 0.71880381, - 0.61168231, - 0.47492593, - -0.04877796, - -0.29539943, - -0.57208567, - -1.29065614, + 1.55277912, + 1.64601545, + 1.70149005, + 1.75074821, + 1.7729181, + 1.73988943, + 1.66962305, + 1.32169463, + 1.13725974, + 0.93489343, + 0.33248456, + 0.09385697, + -0.24597585, + -1.18928361, -1.54, ], [ 1.54, - 1.53171625, - 1.43067973, - 1.3649996, - 1.29810734, - 1.10897703, - 1.0374163, - 0.93992459, - 0.52348463, - 0.31612114, - 0.12283504, - -0.14736347, - -0.15187642, - -0.15372295, - -0.06603706, + 1.51288542, + 1.24586886, + 1.07751375, + 0.89766314, + 0.36810254, + 0.16028788, + -0.06376381, + -0.64591559, + -0.84802856, + -1.02114973, + -1.22319089, + -1.19797412, + -0.94296141, + -0.25131073, 0.0, ], ] @@ -434,38 +447,38 @@ def test_example_two_pendulums_algebraic(): [ [ 0.99952583, - 0.99941032, - 0.99681038, - 0.99399033, - 0.98860777, - 0.94010247, - 0.90022299, - 0.84812898, - 0.6584849, - 0.57424555, - 0.45727258, - -0.04875862, - -0.29112201, - -0.5413868, - -0.96101669, + 0.99983769, + 0.99717238, + 0.99147172, + 0.98385231, + 0.97964284, + 0.98573779, + 0.99512061, + 0.96913428, + 0.90748577, + 0.80453579, + 0.32639252, + 0.09371923, + -0.24350292, + -0.92810248, -0.99952583, ], [ -0.03079146, - -0.03433664, - -0.07980643, - -0.10946795, - -0.15051469, - -0.34089199, - -0.43542918, - -0.52978981, - -0.75259394, - -0.81868312, - -0.88932659, - -0.99881059, - -0.95668593, - -0.84077365, - -0.27649037, + -0.01801623, + 0.07514821, + 0.13032198, + 0.17898223, + 0.20074836, + 0.16828846, + 0.09866593, + -0.24653348, + -0.42008283, + -0.59390417, + -0.94523432, + -0.99559867, + -0.96990016, + -0.37232485, -0.03079146, ], ] @@ -479,39 +492,39 @@ def test_example_two_pendulums_algebraic(): [ [ 0.0, - -0.27855252, - -1.03630477, - -1.31246541, - -1.95618328, - -3.72426958, - -4.37488071, - -4.24866308, - -4.22798705, - -4.33906877, - -5.97324789, - -9.14477984, - -9.83232325, - -10.76632382, - -9.97701724, - -8.46520416, + 0.72949413, + 1.96200651, + 2.13477406, + 1.50770278, + -0.86771318, + -1.97956706, + -3.37403326, + -6.66979418, + -7.68799241, + -8.17163582, + -9.21840884, + -9.50105648, + -13.4583828, + -13.77256442, + -10.04523499, ], [ 0.0, - -0.62989128, - -2.28678176, - -2.86981849, - -2.66247711, - -2.79724415, - -3.10324185, - -4.42493009, - -7.59665489, - -8.59683036, - -6.62468483, - -1.17527383, - 0.84182637, - 0.28582148, - 2.24545193, - 4.23600587, + -1.81542041, + -5.89268312, + -7.06202618, + -7.239922, + -8.04717449, + -8.46022852, + -8.71929446, + -8.08597827, + -7.36329269, + -5.7789741, + -0.05345139, + 2.55360354, + 10.03655025, + 9.92968345, + 2.36850482, ], ] ), @@ -542,15 +555,15 @@ def test_example_three_bar(): [ [ 1.3, - 1.28112296, - 1.22535242, - 1.13876849, - 1.02751913, - 0.89519494, - 0.74384486, - 0.57524378, - 0.39190694, - 0.19799513, + 1.285379, + 1.241619, + 1.169068, + 1.068419, + 0.940879, + 0.78836, + 0.613674, + 0.420657, + 0.214183, 0.0, ] ], @@ -581,68 +594,68 @@ def test_example_four_bar(): [ [ 0.77, - 0.76754753, - 0.76031326, - 0.74858082, - 0.73266023, - 0.71288935, - 0.6896361, - 0.66330062, - 0.63432063, - 0.60319444, - 0.5705717, - 0.53750942, - 0.50529314, - 0.47306849, - 0.4387326, - 0.39365688, - 0.33550945, - 0.2727274, - 0.20440837, - 0.13396314, - 0.06433518, - -0.00455076, - -0.07445804, - -0.1471207, - -0.22344713, - -0.30363321, - -0.38765837, - -0.47567902, - -0.56818751, - -0.66598663, + 0.76949359, + 0.76787468, + 0.76492894, + 0.7604129, + 0.75405472, + 0.74555372, + 0.7345782, + 0.72076059, + 0.70368875, + 0.68289166, + 0.65781596, + 0.627787, + 0.59193902, + 0.54907238, + 0.49730222, + 0.43305918, + 0.34900314, + 0.24145408, + 0.12931534, + 0.02311091, + -0.07736037, + -0.17257584, + -0.26263321, + -0.34766303, + -0.42792951, + -0.50378263, + -0.57559366, + -0.64371071, + -0.70843351, -0.77, ], [ 0.0, - -0.0020824, - -0.00821695, - -0.01830149, - -0.03236938, - -0.05048038, - -0.0726346, - -0.09875238, - -0.1287542, - -0.16275482, - -0.20132215, - -0.24535886, - -0.29370214, - -0.33909243, - -0.37268925, - -0.39294823, - -0.40355002, - -0.39595389, - -0.37802695, - -0.35578337, - -0.32816605, - -0.29365283, - -0.25313732, - -0.20969424, - -0.16682135, - -0.12698646, - -0.09139836, - -0.0605062, - -0.03456109, - -0.01404837, + 0.00033752, + 0.00123364, + 0.00244202, + 0.0036886, + 0.0046699, + 0.0050507, + 0.00446145, + 0.00249508, + -0.00129614, + -0.00740281, + -0.01635682, + -0.02871775, + -0.04502648, + -0.06566813, + -0.09048189, + -0.11763193, + -0.14049351, + -0.14563252, + -0.13032206, + -0.10420413, + -0.07458985, + -0.04603899, + -0.02128835, + -0.00176531, + 0.01196416, + 0.01985102, + 0.02214153, + 0.01924878, + 0.01168099, 0.0, ], ], @@ -712,15 +725,15 @@ def test_example_two_pendulums_2constraint(): [ [ -0.5, - -0.47400672, - -0.39832961, - -0.28240044, - -0.14064867, - 0.01147023, - 0.15989866, - 0.29301816, - 0.40093225, - 0.47362475, + -0.476922, + -0.409191, + -0.301994, + -0.164726, + -0.010378, + 0.145734, + 0.28777, + 0.401256, + 0.474597, 0.5, ] ], @@ -750,17 +763,17 @@ def test_example_two_pendulums_rotule(): states["q_u"], [ [ - 0.52359878, - 0.49719372, - 0.41660698, - 0.28429009, - 0.10904561, - -0.09605531, - -0.31061947, - -0.50569474, - -0.65398428, - -0.74758078, - -0.78539816, + 0.523599, + 0.497571, + 0.41848, + 0.286916, + 0.110176, + -0.09611, + -0.30917, + -0.503139, + -0.654991, + -0.750469, + -0.785398, ] ], decimal=6, @@ -789,102 +802,102 @@ def test_example_arm26_pendulum_swingup(): states["q_u"], [ [ - -6.23253084e-01, - -6.38484167e-01, - -6.68922578e-01, - -6.81792550e-01, - -6.55238890e-01, - -5.87796062e-01, - -4.89966451e-01, - -3.76043646e-01, - -2.59102081e-01, - -1.48895389e-01, - -5.20034953e-02, - 2.53932736e-02, - 7.47309323e-02, - 8.76935179e-02, - 6.12325532e-02, - -1.52507329e-03, - -9.10955428e-02, - -1.94140944e-01, - -2.97712860e-01, - -3.90212593e-01, - -4.61042809e-01, - -5.01160801e-01, - -5.04847569e-01, - -4.71389909e-01, - -4.05573563e-01, - -3.16705448e-01, - -2.16476676e-01, - -1.17080223e-01, - -3.07197021e-02, - 2.99772919e-02, - 5.23003485e-02, + -2.69135824e-01, + -2.64498120e-01, + -2.50200880e-01, + -2.25632471e-01, + -1.90588426e-01, + -1.45509776e-01, + -9.17706792e-02, + -3.18630768e-02, + 2.15488905e-02, + 3.71700043e-02, + -1.47743514e-02, + -1.18632803e-01, + -2.25113739e-01, + -3.08059286e-01, + -3.62876634e-01, + -3.92234880e-01, + -4.01385872e-01, + -3.95867834e-01, + -3.79002763e-01, + -3.50727200e-01, + -3.08796279e-01, + -2.50925016e-01, + -1.76355800e-01, + -8.66290583e-02, + 1.41840915e-02, + 1.19698408e-01, + 2.21938379e-01, + 3.12495334e-01, + 3.83726881e-01, + 4.29493011e-01, + 4.45350909e-01, ], [ - 3.80567092e-01, - 4.51818983e-01, - 6.23662754e-01, - 8.05120614e-01, - 9.30553547e-01, - 9.83693526e-01, - 9.77409213e-01, - 9.35719215e-01, - 8.82442686e-01, - 8.35220491e-01, - 8.04531259e-01, - 8.00362705e-01, - 8.40009103e-01, - 9.41814395e-01, - 1.11224555e00, - 1.34321756e00, - 1.61055797e00, - 1.88083666e00, - 2.12537366e00, - 2.32559227e00, - 2.47363237e00, - 2.57049953e00, - 2.62094933e00, - 2.62962518e00, - 2.60031256e00, - 2.53707423e00, - 2.44633497e00, - 2.33899114e00, - 2.23156343e00, - 2.14666431e00, - 2.11304119e00, + 1.32697117e-04, + 3.85224467e-03, + 1.29670231e-02, + 2.31446707e-02, + 2.98803773e-02, + 2.93092213e-02, + 1.92421639e-02, + 4.25995214e-08, + 1.98533259e-07, + 9.97417517e-02, + 3.78068308e-01, + 7.97638080e-01, + 1.23671744e00, + 1.63341338e00, + 1.97730469e00, + 2.27031542e00, + 2.51487477e00, + 2.71245371e00, + 2.86470161e00, + 2.97447848e00, + 3.04579429e00, + 3.08319634e00, + 3.09131402e00, + 3.07465082e00, + 3.03752423e00, + 2.98423604e00, + 2.91965360e00, + 2.85018330e00, + 2.78474313e00, + 2.73519685e00, + 2.71606423e00, ], [ 0.00000000e00, - 2.19071925e-02, - 8.06926144e-02, - 1.66816334e-01, - 2.70564080e-01, - 3.80497065e-01, - 4.82560923e-01, - 5.63694198e-01, - 6.12524398e-01, - 6.20141635e-01, - 5.80996906e-01, - 4.91848034e-01, - 3.50419451e-01, - 1.56747128e-01, - -8.41634283e-02, - -3.57931966e-01, - -6.41571324e-01, - -9.16060238e-01, - -1.17294667e00, - -1.40983578e00, - -1.62710399e00, - -1.82869131e00, - -2.02079309e00, - -2.20817341e00, - -2.39205176e00, - -2.57114387e00, - -2.74170772e00, - -2.89615124e00, - -3.02328610e00, - -3.11002119e00, + 9.29978638e-03, + 3.61965675e-02, + 7.78802301e-02, + 1.29945901e-01, + 1.86553495e-01, + 2.40873672e-01, + 2.85784980e-01, + 3.15907293e-01, + 3.28332167e-01, + 3.10026789e-01, + 2.28632607e-01, + 7.48871946e-02, + -1.30511638e-01, + -3.68045837e-01, + -6.25640028e-01, + -8.94498746e-01, + -1.16550296e00, + -1.42894095e00, + -1.67707730e00, + -1.90606183e00, + -2.11529404e00, + -2.30561703e00, + -2.47795985e00, + -2.63280431e00, + -2.77016185e00, + -2.88962790e00, + -2.99014292e00, + -3.06928988e00, + -3.12234802e00, -3.14159265e00, ], ], @@ -901,8 +914,8 @@ def test_example_arm26_pendulum_swingup_muscle(): # --- Prepare the ocp --- # ocp, model = arm26_pendulum_swingup_muscle.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/models/arm26_w_pendulum.bioMod", - n_shooting=30, - final_time=1, + n_shooting=10, + final_time=0.5, expand_dynamics=False, ) @@ -912,467 +925,169 @@ def test_example_arm26_pendulum_swingup_muscle(): npt.assert_almost_equal( states["q_u"], - [ - [ - 0.00000000e00, - -3.20620080e-05, - -7.22737278e-04, - -2.95404501e-03, - -5.63924692e-03, - -6.49005147e-03, - -7.44120424e-03, - -1.23041824e-02, - -2.16864947e-02, - -3.11639947e-02, - -3.40183367e-02, - -3.70432103e-02, - -5.02028401e-02, - -7.15901095e-02, - -9.11251000e-02, - -9.67782100e-02, - -1.02636806e-01, - -1.26605153e-01, - -1.62430055e-01, - -1.93215657e-01, - -2.01885869e-01, - -2.10736867e-01, - -2.45435943e-01, - -2.94093000e-01, - -3.33840955e-01, - -3.44774564e-01, - -3.55823516e-01, - -3.98072073e-01, - -4.54910190e-01, - -4.99606632e-01, - -5.11656422e-01, - -5.23739586e-01, - -5.69147128e-01, - -6.28166932e-01, - -6.72816216e-01, - -6.84573949e-01, - -6.96250019e-01, - -7.39135696e-01, - -7.92336556e-01, - -8.30465979e-01, - -8.40175985e-01, - -8.49677979e-01, - -8.83314439e-01, - -9.21855611e-01, - -9.46785879e-01, - -9.52697267e-01, - -9.58289149e-01, - -9.76331668e-01, - -9.92490110e-01, - -9.98834344e-01, - -9.99597271e-01, - -9.99995036e-01, - -9.98551302e-01, - -9.89799356e-01, - -9.77968499e-01, - -9.74086523e-01, - -9.69981207e-01, - -9.53456348e-01, - -9.29990775e-01, - -9.11227010e-01, - -9.06184652e-01, - -9.01122130e-01, - -8.81898582e-01, - -8.56759436e-01, - -8.37838636e-01, - -8.32883913e-01, - -8.27946707e-01, - -8.09404808e-01, - -7.85340850e-01, - -7.67085834e-01, - -7.62252280e-01, - -7.57419485e-01, - -7.39182477e-01, - -7.15118888e-01, - -6.96416053e-01, - -6.91386302e-01, - -6.86330724e-01, - -6.67071745e-01, - -6.41203964e-01, - -6.20752421e-01, - -6.15205311e-01, - -6.09614819e-01, - -5.88228319e-01, - -5.59333655e-01, - -5.36409182e-01, - -5.30185937e-01, - -5.23914212e-01, - -4.99944540e-01, - -4.67660510e-01, - -4.42166181e-01, - -4.35267039e-01, - -4.28324533e-01, - -4.01895309e-01, - -3.66576993e-01, - -3.38924873e-01, - -3.31478891e-01, - -3.24002617e-01, - -2.95697827e-01, - -2.58261881e-01, - -2.29261624e-01, - -2.21498827e-01, - -2.13725121e-01, - -1.84491458e-01, - -1.46302787e-01, - -1.17083535e-01, - -1.09314554e-01, - -1.01558643e-01, - -7.26247823e-02, - -3.53760500e-02, - -7.29152103e-03, - 1.15378024e-04, - 7.48066662e-03, - 3.46628706e-02, - 6.89600515e-02, - 9.42902119e-02, - 1.00893971e-01, - 1.07423714e-01, - 1.31152927e-01, - 1.60235905e-01, - 1.81077138e-01, - 1.86419341e-01, - 1.91658860e-01, - 2.10283440e-01, - 2.32176279e-01, - 2.47191245e-01, - 2.50945089e-01, - 2.54592465e-01, - 2.67307303e-01, - 2.81730339e-01, - 2.91261524e-01, - 2.93592960e-01, - 2.95870417e-01, - 3.04207591e-01, - 3.14515826e-01, - 3.21882445e-01, - 3.23752774e-01, - 3.25632869e-01, - 3.33139282e-01, - 3.43495577e-01, - 3.51382102e-01, - 3.53423056e-01, - 3.55450956e-01, - 3.63029886e-01, - 3.72308078e-01, - 3.78486893e-01, - 3.79950859e-01, - 3.81334667e-01, - 3.85790931e-01, - 3.89592744e-01, - 3.90787335e-01, - 3.90843002e-01, - ], - [ - 1.57079633e00, - 1.57070683e00, - 1.56877430e00, - 1.56244859e00, - 1.55465633e00, - 1.55214408e00, - 1.54963079e00, - 1.54182624e00, - 1.53548604e00, - 1.53352726e00, - 1.53342243e00, - 1.53358406e00, - 1.53736159e00, - 1.54960770e00, - 1.56434639e00, - 1.56902273e00, - 1.57404700e00, - 1.59627475e00, - 1.63279146e00, - 1.66601729e00, - 1.67557322e00, - 1.68532662e00, - 1.72284498e00, - 1.77351446e00, - 1.81321380e00, - 1.82386568e00, - 1.83439811e00, - 1.87157611e00, - 1.91428524e00, - 1.94235745e00, - 1.94912944e00, - 1.95552450e00, - 1.97548072e00, - 1.99181022e00, - 1.99678133e00, - 1.99698930e00, - 1.99672126e00, - 1.99142238e00, - 1.97425977e00, - 1.95318876e00, - 1.94640747e00, - 1.93912560e00, - 1.90723862e00, - 1.85469384e00, - 1.80586461e00, - 1.79157553e00, - 1.77675767e00, - 1.71650563e00, - 1.62652338e00, - 1.54835936e00, - 1.52611510e00, - 1.50340802e00, - 1.41535923e00, - 1.29372722e00, - 1.19538561e00, - 1.16845027e00, - 1.14150204e00, - 1.04306198e00, - 9.21943120e-01, - 8.35634220e-01, - 8.13697318e-01, - 7.92169294e-01, - 7.14666065e-01, - 6.22231124e-01, - 5.58760093e-01, - 5.42994035e-01, - 5.27638109e-01, - 4.73047855e-01, - 4.09347337e-01, - 3.66429173e-01, - 3.55855200e-01, - 3.45601623e-01, - 3.09610923e-01, - 2.68406366e-01, - 2.41017169e-01, - 2.34301495e-01, - 2.27807912e-01, - 2.05218281e-01, - 1.79715374e-01, - 1.62960615e-01, - 1.58876766e-01, - 1.54942802e-01, - 1.41427677e-01, - 1.26563232e-01, - 1.17118160e-01, - 1.14868968e-01, - 1.12728071e-01, - 1.05624097e-01, - 9.84557299e-02, - 9.44808344e-02, - 9.36358519e-02, - 9.28804178e-02, - 9.08415615e-02, - 9.00335035e-02, - 9.08065201e-02, - 9.12144976e-02, - 9.17104157e-02, - 9.43848689e-02, - 9.98127571e-02, - 1.05468918e-01, - 1.07196552e-01, - 1.09024400e-01, - 1.16850090e-01, - 1.29379581e-01, - 1.40759110e-01, - 1.44048120e-01, - 1.47457779e-01, - 1.61451462e-01, - 1.82532621e-01, - 2.00814099e-01, - 2.05989285e-01, - 2.11314763e-01, - 2.32853359e-01, - 2.64595386e-01, - 2.91635511e-01, - 2.99224638e-01, - 3.07008184e-01, - 3.38258820e-01, - 3.83750693e-01, - 4.22063757e-01, - 4.32751871e-01, - 4.43680627e-01, - 4.87198283e-01, - 5.49624470e-01, - 6.01465572e-01, - 6.15820485e-01, - 6.30412117e-01, - 6.87375487e-01, - 7.66460244e-01, - 8.30283417e-01, - 8.47710340e-01, - 8.65207631e-01, - 9.30593835e-01, - 1.01512930e00, - 1.07926290e00, - 1.09625261e00, - 1.11301976e00, - 1.17246183e00, - 1.24233209e00, - 1.29052647e00, - 1.30263879e00, - 1.31436154e00, - 1.35418626e00, - 1.39682255e00, - 1.42284132e00, - 1.42884489e00, - 1.43444363e00, - 1.45171211e00, - 1.46561777e00, - 1.46984373e00, - 1.47003907e00, - ], + np.array( [ - 0.00000000e00, - -3.69853872e-05, - -8.38371941e-04, - -3.46533231e-03, - -6.66746507e-03, - -7.68849847e-03, - -8.83255584e-03, - -1.46985317e-02, - -2.60077458e-02, - -3.73719424e-02, - -4.07793805e-02, - -4.43777462e-02, - -5.98551189e-02, - -8.44521151e-02, - -1.06360344e-01, - -1.12604114e-01, - -1.19028804e-01, - -1.44831438e-01, - -1.82084258e-01, - -2.12994678e-01, - -2.21534824e-01, - -2.30194881e-01, - -2.63716563e-01, - -3.09709927e-01, - -3.46544679e-01, - -3.56576492e-01, - -3.66702404e-01, - -4.05603813e-01, - -4.58428783e-01, - -5.00379502e-01, - -5.11750893e-01, - -5.23208283e-01, - -5.66999155e-01, - -6.25948082e-01, - -6.72529109e-01, - -6.85155148e-01, - -6.97865587e-01, - -7.46286656e-01, - -8.11049720e-01, - -8.61690577e-01, - -8.75295808e-01, - -8.88942417e-01, - -9.40485507e-01, - -1.00830794e00, - -1.06071058e00, - -1.07475359e00, - -1.08883098e00, - -1.14196938e00, - -1.21265328e00, - -1.26859526e00, - -1.28385569e00, - -1.29919882e00, - -1.35686588e00, - -1.43254040e00, - -1.49101266e00, - -1.50666700e00, - -1.52224141e00, - -1.57892342e00, - -1.64847044e00, - -1.69837862e00, - -1.71119877e00, - -1.72384419e00, - -1.76992481e00, - -1.82724101e00, - -1.86944804e00, - -1.88048734e00, - -1.89144961e00, - -1.93200404e00, - -1.98362784e00, - -2.02229775e00, - -2.03247896e00, - -2.04261083e00, - -2.08020291e00, - -2.12833957e00, - -2.16465355e00, - -2.17425823e00, - -2.18383389e00, - -2.21952096e00, - -2.26555730e00, - -2.30048373e00, - -2.30973997e00, - -2.31897454e00, - -2.35343101e00, - -2.39788741e00, - -2.43154523e00, - -2.44044773e00, - -2.44932129e00, - -2.48235304e00, - -2.52475640e00, - -2.55667488e00, - -2.56508894e00, - -2.57346403e00, - -2.60454108e00, - -2.64420153e00, - -2.67387865e00, - -2.68167631e00, - -2.68942782e00, - -2.71810862e00, - -2.75451948e00, - -2.78161920e00, - -2.78871806e00, - -2.79576644e00, - -2.82177689e00, - -2.85463030e00, - -2.87893907e00, - -2.88528286e00, - -2.89157116e00, - -2.91468371e00, - -2.94362591e00, - -2.96481876e00, - -2.97031336e00, - -2.97574440e00, - -2.99556735e00, - -3.02003833e00, - -3.03766100e00, - -3.04218284e00, - -3.04663166e00, - -3.06267949e00, - -3.08200669e00, - -3.09551393e00, - -3.09891340e00, - -3.10222757e00, - -3.11388962e00, - -3.12716503e00, - -3.13576099e00, - -3.13781028e00, - -3.13975605e00, - -3.14610970e00, - -3.15207499e00, - -3.15481161e00, - -3.15526781e00, - -3.15562439e00, - -3.15622576e00, - -3.15530450e00, - -3.15332736e00, - -3.15261784e00, - -3.15188007e00, - -3.14930693e00, - -3.14646739e00, - -3.14463929e00, - -3.14418617e00, - -3.14377182e00, - -3.14277291e00, - -3.14226273e00, - -3.14204669e00, - -3.14197530e00, - -3.14190633e00, - -3.14171569e00, - -3.14160546e00, - -3.14159152e00, - -3.14159265e00, - ], - ], + [ + 0.00000000e00, + -2.42756052e-04, + -5.46176583e-03, + -2.21847781e-02, + -4.21321813e-02, + -4.84198776e-02, + -5.50960721e-02, + -8.34624676e-02, + -1.27760701e-01, + -1.66731157e-01, + -1.77784649e-01, + -1.89079118e-01, + -2.33428582e-01, + -2.96177764e-01, + -3.48049081e-01, + -3.62405284e-01, + -3.76968315e-01, + -4.33289684e-01, + -5.10056035e-01, + -5.70526066e-01, + -5.86751923e-01, + -6.02969155e-01, + -6.63239320e-01, + -7.39089606e-01, + -7.93706958e-01, + -8.07584321e-01, + -8.21128855e-01, + -8.68579609e-01, + -9.21245035e-01, + -9.53440226e-01, + -9.60711716e-01, + -9.67408939e-01, + -9.87224385e-01, + -9.99865845e-01, + -9.98922967e-01, + -9.97065237e-01, + -9.94554455e-01, + -9.79770993e-01, + -9.48161004e-01, + -9.14869882e-01, + -9.04713314e-01, + -8.94039400e-01, + -8.49571525e-01, + -7.81682765e-01, + -7.22849648e-01, + -7.06285078e-01, + -6.89327493e-01, + -6.21902196e-01, + -5.26640979e-01, + -4.49650571e-01, + -4.28730524e-01, + ], + [ + 1.57079633e00, + 1.57137136e00, + 1.58369281e00, + 1.62294998e00, + 1.66946377e00, + 1.68406339e00, + 1.69943962e00, + 1.76273318e00, + 1.85667092e00, + 1.93528548e00, + 1.95693953e00, + 1.97846427e00, + 2.05477093e00, + 2.14323978e00, + 2.20200875e00, + 2.21627410e00, + 2.22987552e00, + 2.27437500e00, + 2.31664308e00, + 2.33669175e00, + 2.34019795e00, + 2.34292864e00, + 2.34624788e00, + 2.33405713e00, + 2.31200582e00, + 2.30426711e00, + 2.29575134e00, + 2.25692315e00, + 2.18999079e00, + 2.12617833e00, + 2.10733002e00, + 2.08772018e00, + 2.00744476e00, + 1.88645489e00, + 1.78057455e00, + 1.75032391e00, + 1.71940166e00, + 1.59947266e00, + 1.43309869e00, + 1.29735085e00, + 1.25990646e00, + 1.22196181e00, + 1.07529692e00, + 8.74236824e-01, + 7.13697867e-01, + 6.70161269e-01, + 6.26186086e-01, + 4.56017881e-01, + 2.26017379e-01, + 4.75324708e-02, + 7.24258406e-05, + ], + [ + 0.00000000e00, + -4.27196851e-04, + -9.60627158e-03, + -3.92506859e-02, + -7.49842144e-02, + -8.63260975e-02, + -9.84086081e-02, + -1.50134342e-01, + -2.31553753e-01, + -3.03208060e-01, + -3.23466407e-01, + -3.44136122e-01, + -4.24810117e-01, + -5.36131832e-01, + -6.24817435e-01, + -6.48793311e-01, + -6.72870698e-01, + -7.63799235e-01, + -8.83041942e-01, + -9.74505769e-01, + -9.98879953e-01, + -1.02323493e00, + -1.11429676e00, + -1.23306215e00, + -1.32486373e00, + -1.34953595e00, + -1.37429135e00, + -1.46791512e00, + -1.59219121e00, + -1.68924236e00, + -1.71535763e00, + -1.74154439e00, + -1.84011621e00, + -1.96904640e00, + -2.06771744e00, + -2.09393944e00, + -2.12002468e00, + -2.21589458e00, + -2.33671140e00, + -2.42662283e00, + -2.45025744e00, + -2.47375316e00, + -2.56077280e00, + -2.67266785e00, + -2.75858852e00, + -2.78170366e00, + -2.80493688e00, + -2.89330543e00, + -3.01414149e00, + -3.11372577e00, + -3.14159265e00, + ], + ] + ), decimal=6, ) @@ -1386,8 +1101,8 @@ def test_example_arm26_pendulum_swingup_muscle_algebraic(): # --- Prepare the ocp --- # ocp, model = arm26_pendulum_swingup_muscle_algebraic.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/models/arm26_w_pendulum.bioMod", - n_shooting=30, - final_time=1, + n_shooting=10, + final_time=0.5, expand_dynamics=False, ) @@ -1397,466 +1112,168 @@ def test_example_arm26_pendulum_swingup_muscle_algebraic(): npt.assert_almost_equal( states["q_u"], - [ - [ - 0.00000000e00, - -3.20620080e-05, - -7.22737278e-04, - -2.95404501e-03, - -5.63924692e-03, - -6.49005147e-03, - -7.44120424e-03, - -1.23041824e-02, - -2.16864947e-02, - -3.11639947e-02, - -3.40183367e-02, - -3.70432103e-02, - -5.02028401e-02, - -7.15901095e-02, - -9.11251000e-02, - -9.67782100e-02, - -1.02636806e-01, - -1.26605153e-01, - -1.62430055e-01, - -1.93215657e-01, - -2.01885869e-01, - -2.10736867e-01, - -2.45435943e-01, - -2.94093000e-01, - -3.33840955e-01, - -3.44774564e-01, - -3.55823516e-01, - -3.98072073e-01, - -4.54910190e-01, - -4.99606632e-01, - -5.11656422e-01, - -5.23739586e-01, - -5.69147128e-01, - -6.28166932e-01, - -6.72816216e-01, - -6.84573949e-01, - -6.96250019e-01, - -7.39135696e-01, - -7.92336556e-01, - -8.30465979e-01, - -8.40175985e-01, - -8.49677979e-01, - -8.83314439e-01, - -9.21855611e-01, - -9.46785879e-01, - -9.52697267e-01, - -9.58289149e-01, - -9.76331668e-01, - -9.92490110e-01, - -9.98834344e-01, - -9.99597271e-01, - -9.99995036e-01, - -9.98551302e-01, - -9.89799356e-01, - -9.77968499e-01, - -9.74086523e-01, - -9.69981207e-01, - -9.53456348e-01, - -9.29990775e-01, - -9.11227010e-01, - -9.06184652e-01, - -9.01122130e-01, - -8.81898582e-01, - -8.56759436e-01, - -8.37838636e-01, - -8.32883913e-01, - -8.27946707e-01, - -8.09404808e-01, - -7.85340850e-01, - -7.67085834e-01, - -7.62252280e-01, - -7.57419485e-01, - -7.39182477e-01, - -7.15118888e-01, - -6.96416053e-01, - -6.91386302e-01, - -6.86330724e-01, - -6.67071745e-01, - -6.41203964e-01, - -6.20752421e-01, - -6.15205311e-01, - -6.09614819e-01, - -5.88228319e-01, - -5.59333655e-01, - -5.36409182e-01, - -5.30185937e-01, - -5.23914212e-01, - -4.99944540e-01, - -4.67660510e-01, - -4.42166181e-01, - -4.35267039e-01, - -4.28324533e-01, - -4.01895309e-01, - -3.66576993e-01, - -3.38924873e-01, - -3.31478891e-01, - -3.24002617e-01, - -2.95697827e-01, - -2.58261881e-01, - -2.29261624e-01, - -2.21498827e-01, - -2.13725121e-01, - -1.84491458e-01, - -1.46302787e-01, - -1.17083535e-01, - -1.09314554e-01, - -1.01558643e-01, - -7.26247823e-02, - -3.53760500e-02, - -7.29152103e-03, - 1.15378024e-04, - 7.48066662e-03, - 3.46628706e-02, - 6.89600515e-02, - 9.42902119e-02, - 1.00893971e-01, - 1.07423714e-01, - 1.31152927e-01, - 1.60235905e-01, - 1.81077138e-01, - 1.86419341e-01, - 1.91658860e-01, - 2.10283440e-01, - 2.32176279e-01, - 2.47191245e-01, - 2.50945089e-01, - 2.54592465e-01, - 2.67307303e-01, - 2.81730339e-01, - 2.91261524e-01, - 2.93592960e-01, - 2.95870417e-01, - 3.04207591e-01, - 3.14515826e-01, - 3.21882445e-01, - 3.23752774e-01, - 3.25632869e-01, - 3.33139282e-01, - 3.43495577e-01, - 3.51382102e-01, - 3.53423056e-01, - 3.55450956e-01, - 3.63029886e-01, - 3.72308078e-01, - 3.78486893e-01, - 3.79950859e-01, - 3.81334667e-01, - 3.85790931e-01, - 3.89592744e-01, - 3.90787335e-01, - 3.90843002e-01, - ], - [ - 1.57079633e00, - 1.57070683e00, - 1.56877430e00, - 1.56244859e00, - 1.55465633e00, - 1.55214408e00, - 1.54963079e00, - 1.54182624e00, - 1.53548604e00, - 1.53352726e00, - 1.53342243e00, - 1.53358406e00, - 1.53736159e00, - 1.54960770e00, - 1.56434639e00, - 1.56902273e00, - 1.57404700e00, - 1.59627475e00, - 1.63279146e00, - 1.66601729e00, - 1.67557322e00, - 1.68532662e00, - 1.72284498e00, - 1.77351446e00, - 1.81321380e00, - 1.82386568e00, - 1.83439811e00, - 1.87157611e00, - 1.91428524e00, - 1.94235745e00, - 1.94912944e00, - 1.95552450e00, - 1.97548072e00, - 1.99181022e00, - 1.99678133e00, - 1.99698930e00, - 1.99672126e00, - 1.99142238e00, - 1.97425977e00, - 1.95318876e00, - 1.94640747e00, - 1.93912560e00, - 1.90723862e00, - 1.85469384e00, - 1.80586461e00, - 1.79157553e00, - 1.77675767e00, - 1.71650563e00, - 1.62652338e00, - 1.54835936e00, - 1.52611510e00, - 1.50340802e00, - 1.41535923e00, - 1.29372722e00, - 1.19538561e00, - 1.16845027e00, - 1.14150204e00, - 1.04306198e00, - 9.21943120e-01, - 8.35634220e-01, - 8.13697318e-01, - 7.92169294e-01, - 7.14666065e-01, - 6.22231124e-01, - 5.58760093e-01, - 5.42994035e-01, - 5.27638109e-01, - 4.73047855e-01, - 4.09347337e-01, - 3.66429173e-01, - 3.55855200e-01, - 3.45601623e-01, - 3.09610923e-01, - 2.68406366e-01, - 2.41017169e-01, - 2.34301495e-01, - 2.27807912e-01, - 2.05218281e-01, - 1.79715374e-01, - 1.62960615e-01, - 1.58876766e-01, - 1.54942802e-01, - 1.41427677e-01, - 1.26563232e-01, - 1.17118160e-01, - 1.14868968e-01, - 1.12728071e-01, - 1.05624097e-01, - 9.84557299e-02, - 9.44808344e-02, - 9.36358519e-02, - 9.28804178e-02, - 9.08415615e-02, - 9.00335035e-02, - 9.08065201e-02, - 9.12144976e-02, - 9.17104157e-02, - 9.43848689e-02, - 9.98127571e-02, - 1.05468918e-01, - 1.07196552e-01, - 1.09024400e-01, - 1.16850090e-01, - 1.29379581e-01, - 1.40759110e-01, - 1.44048120e-01, - 1.47457779e-01, - 1.61451462e-01, - 1.82532621e-01, - 2.00814099e-01, - 2.05989285e-01, - 2.11314763e-01, - 2.32853359e-01, - 2.64595386e-01, - 2.91635511e-01, - 2.99224638e-01, - 3.07008184e-01, - 3.38258820e-01, - 3.83750693e-01, - 4.22063757e-01, - 4.32751871e-01, - 4.43680627e-01, - 4.87198283e-01, - 5.49624470e-01, - 6.01465572e-01, - 6.15820485e-01, - 6.30412117e-01, - 6.87375487e-01, - 7.66460244e-01, - 8.30283417e-01, - 8.47710340e-01, - 8.65207631e-01, - 9.30593835e-01, - 1.01512930e00, - 1.07926290e00, - 1.09625261e00, - 1.11301976e00, - 1.17246183e00, - 1.24233209e00, - 1.29052647e00, - 1.30263879e00, - 1.31436154e00, - 1.35418626e00, - 1.39682255e00, - 1.42284132e00, - 1.42884489e00, - 1.43444363e00, - 1.45171211e00, - 1.46561777e00, - 1.46984373e00, - 1.47003907e00, - ], + np.array( [ - 0.00000000e00, - -3.69853872e-05, - -8.38371941e-04, - -3.46533231e-03, - -6.66746507e-03, - -7.68849847e-03, - -8.83255584e-03, - -1.46985317e-02, - -2.60077458e-02, - -3.73719424e-02, - -4.07793805e-02, - -4.43777462e-02, - -5.98551189e-02, - -8.44521151e-02, - -1.06360344e-01, - -1.12604114e-01, - -1.19028804e-01, - -1.44831438e-01, - -1.82084258e-01, - -2.12994678e-01, - -2.21534824e-01, - -2.30194881e-01, - -2.63716563e-01, - -3.09709927e-01, - -3.46544679e-01, - -3.56576492e-01, - -3.66702404e-01, - -4.05603813e-01, - -4.58428783e-01, - -5.00379502e-01, - -5.11750893e-01, - -5.23208283e-01, - -5.66999155e-01, - -6.25948082e-01, - -6.72529109e-01, - -6.85155148e-01, - -6.97865587e-01, - -7.46286656e-01, - -8.11049720e-01, - -8.61690577e-01, - -8.75295808e-01, - -8.88942417e-01, - -9.40485507e-01, - -1.00830794e00, - -1.06071058e00, - -1.07475359e00, - -1.08883098e00, - -1.14196938e00, - -1.21265328e00, - -1.26859526e00, - -1.28385569e00, - -1.29919882e00, - -1.35686588e00, - -1.43254040e00, - -1.49101266e00, - -1.50666700e00, - -1.52224141e00, - -1.57892342e00, - -1.64847044e00, - -1.69837862e00, - -1.71119877e00, - -1.72384419e00, - -1.76992481e00, - -1.82724101e00, - -1.86944804e00, - -1.88048734e00, - -1.89144961e00, - -1.93200404e00, - -1.98362784e00, - -2.02229775e00, - -2.03247896e00, - -2.04261083e00, - -2.08020291e00, - -2.12833957e00, - -2.16465355e00, - -2.17425823e00, - -2.18383389e00, - -2.21952096e00, - -2.26555730e00, - -2.30048373e00, - -2.30973997e00, - -2.31897454e00, - -2.35343101e00, - -2.39788741e00, - -2.43154523e00, - -2.44044773e00, - -2.44932129e00, - -2.48235304e00, - -2.52475640e00, - -2.55667488e00, - -2.56508894e00, - -2.57346403e00, - -2.60454108e00, - -2.64420153e00, - -2.67387865e00, - -2.68167631e00, - -2.68942782e00, - -2.71810862e00, - -2.75451948e00, - -2.78161920e00, - -2.78871806e00, - -2.79576644e00, - -2.82177689e00, - -2.85463030e00, - -2.87893907e00, - -2.88528286e00, - -2.89157116e00, - -2.91468371e00, - -2.94362591e00, - -2.96481876e00, - -2.97031336e00, - -2.97574440e00, - -2.99556735e00, - -3.02003833e00, - -3.03766100e00, - -3.04218284e00, - -3.04663166e00, - -3.06267949e00, - -3.08200669e00, - -3.09551393e00, - -3.09891340e00, - -3.10222757e00, - -3.11388962e00, - -3.12716503e00, - -3.13576099e00, - -3.13781028e00, - -3.13975605e00, - -3.14610970e00, - -3.15207499e00, - -3.15481161e00, - -3.15526781e00, - -3.15562439e00, - -3.15622576e00, - -3.15530450e00, - -3.15332736e00, - -3.15261784e00, - -3.15188007e00, - -3.14930693e00, - -3.14646739e00, - -3.14463929e00, - -3.14418617e00, - -3.14377182e00, - -3.14277291e00, - -3.14226273e00, - -3.14204669e00, - -3.14197530e00, - -3.14190633e00, - -3.14171569e00, - -3.14160546e00, - -3.14159152e00, - -3.14159265e00, - ], - ], + [ + 0.00000000e00, + -2.42756110e-04, + -5.46176715e-03, + -2.21847832e-02, + -4.21321905e-02, + -4.84198880e-02, + -5.50960836e-02, + -8.34624820e-02, + -1.27760716e-01, + -1.66731170e-01, + -1.77784661e-01, + -1.89079129e-01, + -2.33428588e-01, + -2.96177762e-01, + -3.48049071e-01, + -3.62405272e-01, + -3.76968301e-01, + -4.33289661e-01, + -5.10056000e-01, + -5.70526023e-01, + -5.86751877e-01, + -6.02969107e-01, + -6.63239264e-01, + -7.39089543e-01, + -7.93706893e-01, + -8.07584255e-01, + -8.21128789e-01, + -8.68579545e-01, + -9.21244978e-01, + -9.53440179e-01, + -9.60711673e-01, + -9.67408899e-01, + -9.87224361e-01, + -9.99865847e-01, + -9.98922993e-01, + -9.97065270e-01, + -9.94554495e-01, + -9.79771058e-01, + -9.48161094e-01, + -9.14869985e-01, + -9.04713420e-01, + -8.94039507e-01, + -8.49571634e-01, + -7.81682865e-01, + -7.22849731e-01, + -7.06285155e-01, + -6.89327563e-01, + -6.21902232e-01, + -5.26640956e-01, + -4.49650496e-01, + -4.28730435e-01, + ], + [ + 1.57079633e00, + 1.57137136e00, + 1.58369281e00, + 1.62295000e00, + 1.66946382e00, + 1.68406345e00, + 1.69943968e00, + 1.76273327e00, + 1.85667104e00, + 1.93528561e00, + 1.95693966e00, + 1.97846441e00, + 2.05477108e00, + 2.14323995e00, + 2.20200892e00, + 2.21627428e00, + 2.22987570e00, + 2.27437518e00, + 2.31664328e00, + 2.33669197e00, + 2.34019818e00, + 2.34292887e00, + 2.34624813e00, + 2.33405741e00, + 2.31200614e00, + 2.30426743e00, + 2.29575167e00, + 2.25692351e00, + 2.18999120e00, + 2.12617878e00, + 2.10733049e00, + 2.08772066e00, + 2.00744528e00, + 1.88645547e00, + 1.78057519e00, + 1.75032457e00, + 1.71940233e00, + 1.59947338e00, + 1.43309944e00, + 1.29735160e00, + 1.25990721e00, + 1.22196255e00, + 1.07529763e00, + 8.74237477e-01, + 7.13698444e-01, + 6.70161821e-01, + 6.26186611e-01, + 4.56018288e-01, + 2.26017593e-01, + 4.75325166e-02, + 7.24259565e-05, + ], + [ + 0.00000000e00, + -4.27196950e-04, + -9.60627378e-03, + -3.92506948e-02, + -7.49842313e-02, + -8.63261170e-02, + -9.84086301e-02, + -1.50134373e-01, + -2.31553795e-01, + -3.03208108e-01, + -3.23466456e-01, + -3.44136172e-01, + -4.24810172e-01, + -5.36131892e-01, + -6.24817496e-01, + -6.48793372e-01, + -6.72870760e-01, + -7.63799297e-01, + -8.83042001e-01, + -9.74505824e-01, + -9.98880006e-01, + -1.02323498e00, + -1.11429680e00, + -1.23306218e00, + -1.32486375e00, + -1.34953597e00, + -1.37429136e00, + -1.46791512e00, + -1.59219119e00, + -1.68924232e00, + -1.71535759e00, + -1.74154434e00, + -1.84011615e00, + -1.96904633e00, + -2.06771737e00, + -2.09393936e00, + -2.12002461e00, + -2.21589452e00, + -2.33671134e00, + -2.42662279e00, + -2.45025741e00, + -2.47375313e00, + -2.56077279e00, + -2.67266785e00, + -2.75858853e00, + -2.78170367e00, + -2.80493689e00, + -2.89330544e00, + -3.01414149e00, + -3.11372577e00, + -3.14159265e00, + ], + ] + ), decimal=6, ) diff --git a/tests/shard6/test_vector_layout.py b/tests/shard6/test_vector_layout.py index b459b2af0..0920e2ed4 100644 --- a/tests/shard6/test_vector_layout.py +++ b/tests/shard6/test_vector_layout.py @@ -554,7 +554,7 @@ def test_vector_layout_algebraic_states(): bioptim_folder = TestUtils.bioptim_folder() # --- Prepare the ocp --- # - ocp, model = two_pendulums_algebraic.prepare_ocp( + ocp = two_pendulums_algebraic.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/models/two_pendulums.bioMod", n_shooting=5, final_time=1,