diff --git a/.github/workflows/run_tests_linux.yml b/.github/workflows/run_tests_linux.yml index 407b6da0c..3b7ba46ba 100644 --- a/.github/workflows/run_tests_linux.yml +++ b/.github/workflows/run_tests_linux.yml @@ -36,8 +36,8 @@ jobs: conda list - name: Install extra dependencies - run: | - conda install pip pytest-cov black pytest pytest-cov codecov packaging pytest-mpl -cconda-forge + run: | + conda install "numpy>=2.4,<3" pip pytest-cov black pytest pytest-cov codecov packaging pytest-mpl -cconda-forge sudo apt install -y librhash-dev - name: Install ACADOS on Linux diff --git a/.github/workflows/run_tests_osx_win.yml b/.github/workflows/run_tests_osx_win.yml index 8a6a14fa0..0b2fdf444 100644 --- a/.github/workflows/run_tests_osx_win.yml +++ b/.github/workflows/run_tests_osx_win.yml @@ -50,7 +50,7 @@ jobs: conda list - name: Install extra dependencies - run: conda install pip pytest-cov black pytest pytest-cov codecov packaging -cconda-forge + run: conda install "numpy>=2.4,<3" pip pytest-cov black pytest pytest-cov codecov packaging -cconda-forge - name: Install ACADOS on Mac run: | diff --git a/.vscode/launch.json b/.vscode/launch.json index 9c19c47c2..a52e4cef4 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -5,51 +5,39 @@ "version": "0.2.0", "configurations": [ { - "name": "Run examples GUI", + "name": "Getting Started", "type": "debugpy", "request": "launch", - "module": "bioptim.examples", + "program": "${workspaceFolder}/bioptim/examples/getting_started/basic_ocp.py", + "console": "integratedTerminal", "justMyCode": true, "env": { "PYTHONPATH": "${workspaceFolder}" }, - "cwd": "${fileDirname}" + "cwd": "${workspaceFolder}/bioptim/examples/getting_started/" }, { - "name": "Python : fichier actif", + "name": "Run examples GUI", "type": "debugpy", "request": "launch", - "program": "${file}", - "console": "integratedTerminal", - "justMyCode": false, + "module": "bioptim.examples", + "justMyCode": true, "env": { "PYTHONPATH": "${workspaceFolder}" }, "cwd": "${fileDirname}" }, { - "name": "Pendulum", + "name": "Python : fichier actif", "type": "debugpy", "request": "launch", - "program": "${workspaceFolder}/bioptim/examples/getting_started/pendulum.py", + "program": "${file}", "console": "integratedTerminal", - "justMyCode": true, + "justMyCode": false, "env": { "PYTHONPATH": "${workspaceFolder}" }, - "cwd": "${workspaceFolder}/bioptim/examples/getting_started/" + "cwd": "${fileDirname}" }, - { - "name": "Parameter", - "type": "debugpy", - "request": "launch", - "program": "${workspaceFolder}/bioptim/examples/muscle_driven_ocp/custom_parameters.py", - "console": "integratedTerminal", - "justMyCode": true, - "env": { - "PYTHONPATH": "${workspaceFolder}" - }, - "cwd": "${workspaceFolder}/bioptim/examples/muscle_driven_ocp/" - } ] } \ No newline at end of file diff --git a/README.md b/README.md index d8d4e207c..4ef035792 100644 --- a/README.md +++ b/README.md @@ -788,14 +788,15 @@ One can refer to their respective solver's documentation to know which options e The `show_online_optim` parameter can be set to `True` so the graphs nicely update during the optimization with the default values. One can also directly declare `online_optim` as an `OnlineOptim` parameter to customize the behavior of the plotter. Note that `show_online_optim` and `online_optim` are mutually exclusive. -Please also note that `OnlineOptim.MULTIPROCESS` is not available on Windows and only none of them are available on Macos. -To see how to run the server on Windows, please refer to the `getting_started/pendulum.py` example. +Please also note that `OnlineOptim.MULTIPROCESS` is not available on Windows or Macos. +On Macos, the default backend is `OnlineOptim.MULTIPROCESS_SERVER`, while `OnlineOptim.SERVER` remains available if one wants to start `resources/plotting_server.py` manually. +To see how to run the server explicitly, please refer to the `resources/plotting_server.py` example. It is expected to slow down the optimization a bit. `show_options` can be also passed as a dict to the plotter to customize the plotter's behavior. If `online_optim` is set to `SERVER`, then a server must be started manually by instantiating an `PlottingServer` class (see `ressources/plotting_server.py`). The following keys are additional options when using `OnlineOptim.SERVER` and `OnlineOptim.MULTIPROCESS_SERVER`: - `host`: the host to use (default is `localhost`) - - `port`: the port to use (default is `5030`) + - `port`: the port to use (default is `5030` for `OnlineOptim.SERVER` and a random available port for `OnlineOptim.MULTIPROCESS_SERVER`) If you want to see IPOPT's iterations over the course of the resolution of your opc, it is possible using the following: ```python @@ -1720,7 +1721,7 @@ The type of online plotter to use. The accepted values are: NONE: No online plotter. -DEFAULT: Use the default online plotter depending on the OS (MULTIPROCESS on Linux, MULTIPROCESS_SERVER on Windows and NONE on MacOS). +DEFAULT: Use the default online plotter depending on the OS (MULTIPROCESS on Linux, MULTIPROCESS_SERVER on Windows and macOS). MULTIPROCESS: The online plotter is in a separate process. SERVER: The online plotter is in a separate server. MULTIPROCESS_SERVER: The online plotter using the server automatically setup on a separate process. @@ -2006,6 +2007,9 @@ available in the `biorbd` documentation. ### The [example_optimal_time.py](./bioptim/examples/getting_started/example_optimal_time.py) file Examples of time optimization can be found in 'examples/optimal_time_ocp/'. +### The [example_pinocchio.py](./bioptim/examples/getting_started/example_pinocchio.py) file +This example is the exact same as the pendulum example, but with a model defined using the `Pinocchio` backend (instead of the `biorbd` backend). It is designed to show how to use a model defined in Pinocchio instead of biorbd. + ### The [example_simulation.py](./bioptim/examples/getting_started/example_simulation.py) file The first part of this example is a single shooting simulation from initial guesses. It is not an optimal control program. It is merely the simulation of values that is applying the dynamics. diff --git a/bioptim/__init__.py b/bioptim/__init__.py index 37bc87088..7ccf9b7c2 100644 --- a/bioptim/__init__.py +++ b/bioptim/__init__.py @@ -217,6 +217,7 @@ JointAccelerationBiorbdModel, MultiTorqueBiorbdModel, ) +from .models.pinocchio import PinocchioModel, TorquePinocchioModel from .models.protocols.biomodel import BioModel from .models.protocols.holonomic_constraints import HolonomicConstraintsFcn, HolonomicConstraintsList from .models.protocols.stochastic_biomodel import StochasticBioModel diff --git a/bioptim/dynamics/configure_variables.py b/bioptim/dynamics/configure_variables.py index 97beb3983..6ce42fee7 100644 --- a/bioptim/dynamics/configure_variables.py +++ b/bioptim/dynamics/configure_variables.py @@ -2,11 +2,11 @@ from typing import Callable, Any import numpy as np -from casadi import DM, vertcat, Function +from casadi import DM, vertcat, Function, horzcat from .configure_new_variable import NewVariableConfiguration from .fatigue.fatigue_dynamics import FatigueList -from ..misc.enums import PlotType, ContactType +from ..misc.enums import PlotType, ContactType, ControlType from ..misc.fcn_enum import FcnEnum from ..misc.mapping import BiMapping, Mapping from ..models.protocols.stochastic_biomodel import StochasticBioModel @@ -1204,20 +1204,31 @@ def configure_qv(ocp, nlp, **extra_params) -> None: time_span_sym = vertcat(nlp.time_cx, nlp.dt) - nlp.q_v_function = Function( + sym_qv = ( + nlp.controls.scaled.cx + if nlp.control_type == ControlType.CONSTANT + else nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2) + ) + qv_init_w_algebraic = ( + nlp.algebraic_states["q_v"].cx + if "q_v" in nlp.algebraic_states.keys() + else DM.zeros(nlp.model.nb_dependent_joints, 1) + ) + + q_v_plot_function = Function( "qv_function", [ time_span_sym, - nlp.states.cx, - nlp.controls.cx, - nlp.parameters.cx, - nlp.algebraic_states.cx, + nlp.states.scaled.cx, + sym_qv, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, nlp.numerical_timeseries.cx, ], [ nlp.model.compute_q_v()( nlp.states["q_u"].cx, - DM.zeros(nlp.model.nb_dependent_joints, 1), + qv_init_w_algebraic, ) ], ["t_span", "x", "u", "p", "a", "d"], @@ -1239,7 +1250,7 @@ def configure_qv(ocp, nlp, **extra_params) -> None: ) nlp.plot["q_v"] = CustomPlot( - lambda t0, phases_dt, node_idx, x, u, p, a, d: nlp.q_v_function( + lambda t0, phases_dt, node_idx, x, u, p, a, d: q_v_plot_function( np.concatenate([t0, t0 + phases_dt[nlp.phase_idx]]), x, u, p, a, d ), plot_type=PlotType.INTEGRATED, @@ -1261,12 +1272,24 @@ def configure_qdotv(ocp, nlp, **extra_params) -> None: """ time_span_sym = vertcat(nlp.time_cx, nlp.dt) - nlp.q_v_function = Function( + + sym_qv = ( + nlp.controls.scaled.cx + if nlp.control_type == ControlType.CONSTANT + else nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2) + ) + qv_init_w_algebraic = ( + nlp.algebraic_states["q_v"].cx + if "q_v" in nlp.algebraic_states.keys() + else DM.zeros(nlp.model.nb_dependent_joints, 1) + ) + + qdot_v_plot_function = Function( "qdot_v_function", [ time_span_sym, nlp.states.scaled.cx, - nlp.controls.scaled.cx, + sym_qv, nlp.parameters.scaled.cx, nlp.algebraic_states.scaled.cx, nlp.numerical_timeseries.cx, @@ -1275,7 +1298,7 @@ def configure_qdotv(ocp, nlp, **extra_params) -> None: nlp.model._compute_qdot_v()( nlp.states.scaled["q_u"].cx, nlp.states.scaled["qdot_u"].cx, - DM.zeros(nlp.model.nb_dependent_joints, 1), + qv_init_w_algebraic, ) ], ["t_span", "x", "u", "p", "a", "d"], @@ -1297,7 +1320,7 @@ def configure_qdotv(ocp, nlp, **extra_params) -> None: ) nlp.plot["qdot_v"] = CustomPlot( - lambda t0, phases_dt, node_idx, x, u, p, a, d: nlp.q_v_function( + lambda t0, phases_dt, node_idx, x, u, p, a, d: qdot_v_plot_function( np.concatenate([t0, t0 + phases_dt[nlp.phase_idx]]), x, u, p, a, d ), plot_type=PlotType.INTEGRATED, @@ -1319,12 +1342,25 @@ def configure_lagrange_multipliers_function(ocp, nlp: NpArrayDictOptional, **ext """ time_span_sym = vertcat(nlp.time_cx, nlp.dt) - nlp.lagrange_multipliers_function = Function( + + sym_qv = ( + nlp.controls.scaled.cx + if nlp.control_type == ControlType.CONSTANT + else nlp.cx.sym("new_control", nlp.controls.scaled.cx.shape[0], 2) + ) + ctrl_cx = sym_qv[:, 1] if nlp.control_type == ControlType.LINEAR_CONTINUOUS else nlp.controls.scaled.cx + qv_init_w_algebraic = ( + nlp.algebraic_states["q_v"].cx + if "q_v" in nlp.algebraic_states.keys() + else DM.zeros(nlp.model.nb_dependent_joints, 1) + ) + + lagrange_multipliers_plot_function = Function( "lagrange_multipliers_function", [ time_span_sym, nlp.states.scaled.cx, - nlp.controls.scaled.cx, + sym_qv, nlp.parameters.scaled.cx, nlp.algebraic_states.scaled.cx, nlp.numerical_timeseries.cx, @@ -1333,8 +1369,11 @@ def configure_lagrange_multipliers_function(ocp, nlp: NpArrayDictOptional, **ext nlp.model.compute_the_lagrangian_multipliers()( nlp.states.scaled["q_u"].cx, nlp.states.scaled["qdot_u"].cx, - DM.zeros(nlp.model.nb_dependent_joints, 1), - DynamicsFunctions.get(nlp.controls["tau"], nlp.controls.scaled.cx), + qv_init_w_algebraic, + DynamicsFunctions.get( + nlp.controls["tau"], + ctrl_cx, + ), ) ], ["t_span", "x", "u", "p", "a", "d"], @@ -1360,7 +1399,7 @@ def configure_lagrange_multipliers_function(ocp, nlp: NpArrayDictOptional, **ext ) nlp.plot["lagrange_multipliers"] = CustomPlot( - lambda t0, phases_dt, node_idx, x, u, p, a, d: nlp.lagrange_multipliers_function( + lambda t0, phases_dt, node_idx, x, u, p, a, d: lagrange_multipliers_plot_function( np.concatenate([t0, t0 + phases_dt[nlp.phase_idx]]), x, u, p, a, d ), plot_type=PlotType.INTEGRATED, diff --git a/bioptim/examples/__main__.py b/bioptim/examples/__main__.py index aac3282ab..4edfb5b6f 100644 --- a/bioptim/examples/__main__.py +++ b/bioptim/examples/__main__.py @@ -51,6 +51,7 @@ ("Example simulation", "example_simulation.py"), ("Example cyclic movement", "example_cyclic_movement.py"), ("Example constraint weight", "custom_constraint_weights.py"), + ("Example Pinocchio", "example_pinocchio.py"), ("How to plot", "how_to_plot.py"), ] ), diff --git a/bioptim/examples/getting_started/basic_ocp.py b/bioptim/examples/getting_started/basic_ocp.py index 6b0df00fe..ab86f36d3 100644 --- a/bioptim/examples/getting_started/basic_ocp.py +++ b/bioptim/examples/getting_started/basic_ocp.py @@ -156,8 +156,7 @@ def main(): ocp.print(to_console=False, to_graph=False) # --- Solve the ocp --- # - # Default is OnlineOptim.MULTIPROCESS on Linux, OnlineOptim.MULTIPROCESS_SERVER on Windows and None on MacOS - # To see the graphs on MacOS, one must run the server manually (see resources/plotting_server.py) + # Default is OnlineOptim.MULTIPROCESS_SERVER on all platforms. solver = Solver.IPOPT(online_optim=OnlineOptim.DEFAULT) # # Show the constraints Jacobian sparsity @@ -180,13 +179,16 @@ def main(): sol.print_cost() # sol.graphs(show_bounds=True, save_name="results.png") - # --- Animate the solution --- # - viewer = "bioviz" - # viewer = "pyorerun" - sol.animate(n_frames=0, viewer=viewer, show_now=True) + # # --- Animate the solution --- # + # # To animate the solution, we can use the animate function. Uncomment the current block + # # to see the animation. + # # 'bioviz' must be installed to use the bioviz viewer. + # # Similarly, 'pyorerun' must be installed to use the pyorerun viewer. + # viewer = "bioviz" # "pyorerun" + # sol.animate(n_frames=0, viewer=viewer, show_now=True) # # --- Saving the solver's output after the optimization --- # - # Here is an example of how we recommend to save the solution. Please note that sol.ocp is not picklable and that sol will be loaded using the current bioptim version, not the version at the time of the generation of the results. + # # Here is an example of how we recommend to save the solution. Please note that sol.ocp is not picklable and that sol will be loaded using the current bioptim version, not the version at the time of the generation of the results. # import pickle # import git # from datetime import date diff --git a/bioptim/examples/getting_started/example_pinocchio.py b/bioptim/examples/getting_started/example_pinocchio.py new file mode 100644 index 000000000..fbf789db8 --- /dev/null +++ b/bioptim/examples/getting_started/example_pinocchio.py @@ -0,0 +1,138 @@ +""" +A very simple yet meaningful optimal control program consisting in a pendulum starting downward and ending upward +while requiring the minimum of generalized forces. The solver is only allowed to move the pendulum sideways. + +This simple example is a good place to start investigating bioptim as it describes the most common dynamics out there +(the joint torque driven), it defines an objective function and some boundaries and initial guesses + +During the optimization process, the graphs are updated real-time (even though it is a bit too fast and short to really +appreciate it). Finally, once it finished optimizing, it animates the model using the optimal solution +""" + +from bioptim import ( + OptimalControlProgram, + DynamicsOptions, + BoundsList, + InitialGuessList, + ObjectiveFcn, + Objective, + OdeSolver, + OdeSolverBase, + Solver, + TorquePinocchioModel, + PhaseDynamics, + OnlineOptim, + OrderingStrategy, + CostType, +) + +from bioptim.examples.utils import ExampleUtils + + +def prepare_ocp( + model_path: str, + final_time: float, + n_shooting: int, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + n_threads: int = 1, + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + ordering_strategy: OrderingStrategy = OrderingStrategy.VARIABLE_MAJOR, +) -> OptimalControlProgram: + """ + The initialization of an ocp + + Parameters + ---------- + model_path: str + The path to the Pinocchio model + final_time: float + The time in second required to perform the task + n_shooting: int + The number of shooting points to define int the direct multiple shooting program + ode_solver: OdeSolverBase = OdeSolver.RK4() + Which type of OdeSolver to use + n_threads: int + The number of threads to use in the paralleling (1 = no parallel computing) + phase_dynamics: PhaseDynamics + If the dynamics equation within a phase is unique or changes at each node. + PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within + a phase. PhaseDynamics.ONE_PER_NODE should also be used when multi-node penalties with more than 3 nodes or with COLLOCATION (cx_intermediate_list) are added to the OCP. + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The OptimalControlProgram ready to be solved + """ + + bio_model = TorquePinocchioModel(model_path) + + # Add objective functions + objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") + + # DynamicsOptions + dynamics = DynamicsOptions(ode_solver=ode_solver, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) + + # Path bounds + x_bounds = BoundsList() + x_bounds["q"] = bio_model.bounds_from_ranges("q") + x_bounds["q"][:, [0, -1]] = 0 # Start and end at 0... + x_bounds["q"][1, -1] = 3.14 # ...but end with pendulum 180 degrees rotated + x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") + x_bounds["qdot"][:, [0, -1]] = 0 # Start and end without any velocity + + # Initial guess (optional since it is 0, we show how to initialize anyway) + x_init = InitialGuessList() + x_init["q"] = [0] * bio_model.nb_q + x_init["qdot"] = [0] * bio_model.nb_qdot + + # Define control path bounds + n_tau = bio_model.nb_tau + u_bounds = BoundsList() + u_bounds["tau"] = [-100] * n_tau, [100] * n_tau # Limit the strength of the pendulum to (-100 to 100)... + u_bounds["tau"][1, :] = 0 # ...but remove the capability to actively rotate + + # Initial guess (optional since it is 0, we show how to initialize anyway) + u_init = InitialGuessList() + u_init["tau"] = [0] * n_tau + + return OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_init=x_init, + u_init=u_init, + x_bounds=x_bounds, + u_bounds=u_bounds, + objective_functions=objective_functions, + use_sx=True, + n_threads=n_threads, + ordering_strategy=ordering_strategy, + ) + + +def main(): + """ + If pendulum is run as a script, it will perform the optimization and animates it + """ + + # --- Prepare the ocp --- # + model_path = ExampleUtils.folder + "/models/pendulum.urdf" + ocp = prepare_ocp(model_path=model_path, final_time=1, n_shooting=400, n_threads=2) + + # --- Solve the ocp --- # + ocp.add_plot_penalty(CostType.ALL) # This will display the objectives and constraints at the current iteration + solver = Solver.IPOPT(online_optim=OnlineOptim.DEFAULT) + sol = ocp.solve(solver) + + # --- Show the results graph --- # + sol.print_cost() + # sol.graphs(show_bounds=True, save_name="results.png") + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/models/pendulum.urdf b/bioptim/examples/models/pendulum.urdf new file mode 100644 index 000000000..ef506e78d --- /dev/null +++ b/bioptim/examples/models/pendulum.urdf @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/bioptim/examples/models/two_cubes_lagrange2D_6DOF.bioMod b/bioptim/examples/models/two_cubes_lagrange2D_6DOF.bioMod new file mode 100644 index 000000000..9f8d361c8 --- /dev/null +++ b/bioptim/examples/models/two_cubes_lagrange2D_6DOF.bioMod @@ -0,0 +1,160 @@ +version 1.0 + +gravity 0 0 -9.81 + +segment root + + marker root_origin + parent root + position 0 0 0 + endmarker + +endsegment + +segment cube0 + parent root + RTinMatrix 0 + RT pi/7 -pi/8 -pi/6 xyz 0 0 0 + mass 1.0 + com 0 0 0 + inertia 1 0 0 + 0 1 0 + 0 0 1 + mesh 0 -1 -1 + mesh 0 0 -1 + mesh 0 0 0 + mesh 0 -1 0 + mesh 0 -1 -1 + mesh 1 -1 -1 + mesh 1 0 -1 + mesh 0 0 -1 + mesh 1 0 -1 + mesh 1 0 0 + mesh 0 0 0 + mesh 1 0 0 + mesh 1 -1 0 + mesh 0 -1 0 + mesh 1 -1 0 + mesh 1 -1 -1 + + rotations xyz + translations xyz + rangesQ + -4*pi 4*pi + -4*pi 4*pi + -4*pi 4*pi + -100 100 + -100 100 + -100 100 +endsegment + + marker cube0_1 + parent cube0 + position 0 0 0 + endmarker + + +segment cube1 + parent root + RTinMatrix 0 + RT 0 0 0 xyz 0 2 0 + mass 1.0 + com 0.1 0.2 0.3 + inertia 1 0 0 + 0 1 0 + 0 0 1 + mesh 0 -1 -1 + mesh 0 0 -1 + mesh 0 0 0 + mesh 0 -1 0 + mesh 0 -1 -1 + mesh 1 -1 -1 + mesh 1 0 -1 + mesh 0 0 -1 + mesh 1 0 -1 + mesh 1 0 0 + mesh 0 0 0 + mesh 1 0 0 + mesh 1 -1 0 + mesh 0 -1 0 + mesh 1 -1 0 + mesh 1 -1 -1 + + rotations xyz + translations xyz + + rangesQ + -4*pi 4*pi + -4*pi 4*pi + -4*pi 4*pi + -100 100 + -100 100 + -100 100 +endsegment + + marker cube1_1 + parent cube1 + position 0 0 0 + endmarker + +segment new_frame + parent cube1 + RTinMatrix 0 + RT 1 0 0 xyz 0 0 0 +endsegment + +segment new_frame2 + parent cube1 + RTinMatrix 1 + RT + 1.000000 0.000000 0.000000 0.000000 + 0.000000 0.000000 -1.000000 0.000000 + 0.000000 1.000000 0.000000 0.000000 + 0.000000 0.000000 0.000000 1.000000 +endsegment + +segment new_frame3 + parent cube1 + RTinMatrix 1 + RT + 0.7500000 -0.4330127 0.5000000 0 + 0.6495190 0.6250000 -0.4330127 0 + -0.1250000 0.6495190 0.7500000 0 + 0.000000 0.000000 0.000000 1.000000 +endsegment + +segment new_frame4 + parent cube1 + RTinMatrix 1 + RT + 1 0 0 0 + 0 -0.5000000 -0.8660254 0 + 0 0.8660254 -0.5000000 0 + 0.000000 0.000000 0.000000 1.000000 +endsegment + +segment new_frame5 + parent cube1 + RTinMatrix 1 + RT + -0.000000 -0.070711 -0.997497 0 + -0.124294 0.989762 -0.070163 0 + 0.992245 0.123983 -0.008789 0 + 0.000000 0.000000 0.000000 1.000000 +endsegment + +segment new_frame6 + parent cube1 + RTinMatrix 1 + RT + 0 1 0 0 + 0 0 1 0 + 1 0 0 0 + 0.000000 0.000000 0.000000 1.000000 +endsegment + +segment new_frame7 + parent cube1 + RTinMatrix 0 + RT 0 pi/2 pi/2 xyz 0 0 0 +endsegment diff --git a/bioptim/examples/models/two_cubes_lagrange2D_outofplane.bioMod b/bioptim/examples/models/two_cubes_lagrange2D_outofplane.bioMod new file mode 100644 index 000000000..b8b9f3da7 --- /dev/null +++ b/bioptim/examples/models/two_cubes_lagrange2D_outofplane.bioMod @@ -0,0 +1,76 @@ +version 1.0 + +gravity 0 0 0.0001 + +segment root + + marker root_origin + parent root + position 0 0 0 + endmarker + +endsegment + +segment cube0 + parent root + RTinMatrix 0 + RT pi/7 -pi/8 -pi/6 xyz 0 0 0 + mass 1.0 + com 0 0 0 + inertia 1 0 0 + 0 1 0 + 0 0 1 + mesh 0 -1 -1 + mesh 0 0 -1 + mesh 0 0 0 + mesh 0 -1 0 + mesh 0 -1 -1 + mesh 1 -1 -1 + mesh 1 0 -1 + mesh 0 0 -1 + mesh 1 0 -1 + mesh 1 0 0 + mesh 0 0 0 + mesh 1 0 0 + mesh 1 -1 0 + mesh 0 -1 0 + mesh 1 -1 0 + mesh 1 -1 -1 + + rotations x + rangesQ + -4*pi 4*pi +endsegment + +segment cube1 + parent root + RTinMatrix 0 + RT 0 0 0 xyz 0 2 0 + mass 1.0 + com 0 0 0 + inertia 1 0 0 + 0 1 0 + 0 0 1 + mesh 0 -1 -1 + mesh 0 0 -1 + mesh 0 0 0 + mesh 0 -1 0 + mesh 0 -1 -1 + mesh 1 -1 -1 + mesh 1 0 -1 + mesh 0 0 -1 + mesh 1 0 -1 + mesh 1 0 0 + mesh 0 0 0 + mesh 1 0 0 + mesh 1 -1 0 + mesh 0 -1 0 + mesh 1 -1 0 + mesh 1 -1 -1 + + rotations xyz + rangesQ + -4*pi 4*pi + -4*pi 4*pi + -4*pi 4*pi +endsegment diff --git a/bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_orientation.py b/bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_orientation.py new file mode 100644 index 000000000..366d24ee8 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_orientation.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +Example: two cubes actuated by torques in all 3 directions, kept parallel by a holonomic +constraint on their orientations (the "align_frames_small_angles" constraint). +This example uses the constraint using only the small angle approximation. + +""" + +import os + +import numpy as np +from casadi import DM + +from bioptim import ( + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + SolutionMerge, + Solver, + InterpolationType, +) +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) + 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 independent_joint_index in bio_model.independent_joint_index: + tau[independent_joint_index, :-1] = controls["tau"][independent_joint_index, :] + for dependent_joint_index in bio_model.dependent_joint_index: + tau[dependent_joint_index, :-1] = controls["tau"][dependent_joint_index, :] + + 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, + final_time: float = 1.0, + expand_dynamics: bool = False, + ode_solver=OdeSolver.RK4(), +): + + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "align_cubes", + HolonomicConstraintsFcn.align_frames_small_angles, + frame_1_idx=1, # segment index of the first cube + frame_2_idx=2, # segment index of the second cube + ) + + bio_model = HolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0], + dependent_joint_index=[1, 2, 3], # rotation of cube 1 + ) + + objectives = ObjectiveList() + objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1) + + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=ode_solver, expand_dynamics=expand_dynamics)) + + x_bounds = BoundsList() + x_bounds.add( + "q_u", + min_bound=np.array([[-100], [-100], [-100]]).T, + max_bound=np.array([[100], [100], [100]]).T, + interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT, + ) + x_bounds.add( + "qdot_u", + min_bound=np.array([[-100], [-100], [-100]]).T, + max_bound=np.array([[100], [100], [100]]).T, + interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT, + ) + + x_bounds["q_u"][:, 0] = [0] + x_bounds["qdot_u"][:, 0] = 0 # no initial velocity + + u_bounds = BoundsList() + u_bounds["tau"] = [1, 0, 0, 0], [1, 0, 0, 0] + + constraints = ConstraintList() + + ocp = OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + objective_functions=objectives, + constraints=constraints, + ) + return ocp, bio_model + + +def main(): + model_folder = os.path.join(ExampleUtils.folder, "models") + model_path = os.path.join(model_folder, "two_cubes_lagrange2D_outofplane.bioMod") + + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path, n_shooting=10, final_time=2.0) + + solver = Solver.IPOPT() + sol = ocp.solve(solver) + + print(f"Optimization finished in {sol.real_time_to_optimize:.2f} s") + + # --- Extract Lagrange multipliers --- + q, _, _, _ = compute_all_states(sol, bio_model) + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.show_global_ref_frame = True + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + 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") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_orientation_6DOF.py b/bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_orientation_6DOF.py new file mode 100644 index 000000000..086e31fe8 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/frame_alignment_orientation_6DOF.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +Example: two cubes actuated by torques and forces along 6 DoF, kept parallel by a holonomic +constraint on their orientations (the "align_frames_generalized" constraint). +This example uses the constraint using the true formulation and the small angle approximation. +It also demonstrates how the user can align the orientation according to different frames in the model. + +""" + +import os + +import numpy as np + +from bioptim import ( + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + SolutionMerge, + Solver, + Node, + BiMappingList, + InitialGuessList, +) +from bioptim.examples.utils import ExampleUtils + +from .custom_dynamics import ModifiedHolonomicTorqueBiorbdModel, constraint_holonomic, constraint_holonomic_end + + +def build_dummy_trajectory_for_the_driving_cube(n_shooting: int): + # Define the three points (each is a 4D vector) + point1 = np.array([0]).T + point2 = np.array([1]).T # + point3 = np.array([0]).T + # Generate interpolation points (0 to 2) + t = np.linspace(0, 2, n_shooting) + + # Interpolate between point1 and point2 (first half) + interp1 = point1 + t[: n_shooting // 2, np.newaxis] * (point2 - point1) + + # Interpolate between point2 and point3 (second half) + interp2 = point2 + t[: n_shooting // 2, np.newaxis] * (point3 - point2) + + # Combine the two interpolations + return np.vstack((interp1, interp2)) + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1.0, + interpolated_points: np.array = None, + expand_dynamics: bool = False, + ode_solver=OdeSolver.COLLOCATION(), + n_threads: int = 1, +): + + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "translation_constraint", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="cube0_1", + marker_2="cube1_1", + index=slice(0, 3), + local_frame_index=1, + ) + + holonomic_constraints.add( + "orientation_constraint", + HolonomicConstraintsFcn.align_frames_orientation, + frame_1_idx=1, + frame_2_idx=2, # The user can changethe frame to align to here + ) + + independant_joints = [0, 1, 2, 3, 4, 5] + computed_joints = [6, 7, 8, 9, 10, 11] + + bio_model = ModifiedHolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=independant_joints, + dependent_joint_index=computed_joints, + ) + print([bio_model.model.segments()[i].name().to_string() for i in range(bio_model.nb_segments)]) + + # Boundaries + u_variable_bimapping = BiMappingList() + u_variable_bimapping.add( + "q", to_second=[0, 1, 2, 3, 4, 5, None, None, None, None, None, None], to_first=independant_joints + ) + u_variable_bimapping.add( + "qdot", to_second=[0, 1, 2, 3, 4, 5, None, None, None, None, None, None], to_first=independant_joints + ) + + v_variable_bimapping = BiMappingList() + v_variable_bimapping.add( + "q", + to_second=[None, None, None, None, None, None, 0, 1, 2, 3, 4, 5], + to_first=computed_joints, + ) + + objectives = ObjectiveList() + objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1) + objectives.add(ObjectiveFcn.Lagrange.TRACK_STATE, key="q_u", index=[0], target=interpolated_points.T, weight=1e4) + objectives.add(ObjectiveFcn.Lagrange.TRACK_STATE, key="q_u", index=[4], target=interpolated_points.T, weight=1e3) + + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=ode_solver, expand_dynamics=expand_dynamics)) + + # Path bounds + x_bounds = BoundsList() + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=u_variable_bimapping) + x_bounds["q_u"][:, 0] = 0 # Start and end positions + + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=u_variable_bimapping) + + u_bounds = BoundsList() + u_bounds["tau"] = [-100, -100, -100, -100, -100, -100, 0, 0, 0, 0, 0, 0], [ + 100, + 100, + 100, + 100, + 100, + 100, + 0, + 0, + 0, + 0, + 0, + 0, + ] + + initial_pos = [0, 0, 0, 0, 0, 0, -1.5, 0, -1.5, 0, 0, 0] + + x_init = InitialGuessList() + x_init["q_u"] = [initial_pos[i] for i in independant_joints] + a_init = InitialGuessList() + a_init.add("q_v", [initial_pos[i] for i in computed_joints]) + + # Path Constraints + constraints = ConstraintList() + constraints.add(constraint_holonomic, node=Node.ALL_SHOOTING) + constraints.add(constraint_holonomic_end, node=Node.END) + + return OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + a_init=a_init, + objective_functions=objectives, + constraints=constraints, + n_threads=n_threads, + ) + + +def main(): + model_folder = os.path.join(ExampleUtils.folder, "models") + model_path = os.path.join(model_folder, "two_cubes_lagrange2D_6DOF.bioMod") + + n_shooting = 10 + + interpolated_points = build_dummy_trajectory_for_the_driving_cube(n_shooting) + + ocp = prepare_ocp( + biorbd_model_path=model_path, + n_shooting=n_shooting, + final_time=1.0, + interpolated_points=interpolated_points, + n_threads=25, + ) + + solver = Solver.IPOPT() + sol = ocp.solve(solver) + + print(f"Optimization finished in {sol.real_time_to_optimize:.2f} s") + + states = sol.decision_states(to_merge=SolutionMerge.NODES) + print(states["q_u"]) + + # --- Extract Lagrange multipliers --- + 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() + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.show_global_ref_frame = True + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + 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") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/gui/graph.py b/bioptim/gui/graph.py index 24d0b727f..695e58c55 100644 --- a/bioptim/gui/graph.py +++ b/bioptim/gui/graph.py @@ -8,8 +8,8 @@ class GraphAbstract: - _return_line: "" - _squared: "" + _return_line = "" + _squared = "" """ Methods ------- @@ -61,6 +61,10 @@ def _vector_layout_structure(self, vector: FloatIterableorNpArray, decimal: Int) """ condensed_vector = "" for i, var in enumerate(vector): + if isinstance(var, np.ndarray): + if var.size > 1: + raise ValueError("The vector layout structure needs to be applied on scalar only") + var = var[0] condensed_vector += f"{round(float(var), decimal):.{decimal}f} " if i % 7 == 0 and i != 0: condensed_vector += f"... {self._return_line}... " diff --git a/bioptim/gui/online_callback_multiprocess.py b/bioptim/gui/online_callback_multiprocess.py index eea4bbbe8..8eef0ea93 100644 --- a/bioptim/gui/online_callback_multiprocess.py +++ b/bioptim/gui/online_callback_multiprocess.py @@ -8,13 +8,7 @@ from .plot import PlotOcp, OcpSerializable from ..optimization.optimization_vector import OptimizationVectorHelper from .online_callback_abstract import OnlineCallbackAbstract -from ..misc.parameters_types import ( - Bool, - Float, - AnyIterable, - AnyDictOptional, - IntListOptional, -) +from ..misc.parameters_types import Bool, Float, AnyIterable, AnyDictOptional, IntListOptional class OnlineCallbackMultiprocess(OnlineCallbackAbstract): diff --git a/bioptim/gui/online_callback_multiprocess_server.py b/bioptim/gui/online_callback_multiprocess_server.py index 49176b4cf..fb0f6e1b9 100644 --- a/bioptim/gui/online_callback_multiprocess_server.py +++ b/bioptim/gui/online_callback_multiprocess_server.py @@ -1,7 +1,10 @@ from multiprocessing import Process +import socket from .online_callback_server import PlottingServer, OnlineCallbackServer +_DEFAULT_HOST = "localhost" + def _start_server_internal(**kwargs): """ @@ -15,6 +18,12 @@ def _start_server_internal(**kwargs): PlottingServer(**kwargs) +def _find_available_tcp_port(host: str) -> int: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + sock.bind((host, 0)) + return sock.getsockname()[1] + + class OnlineCallbackMultiprocessServer(OnlineCallbackServer): def __init__(self, *args, **kwargs): """ @@ -24,8 +33,12 @@ def __init__(self, *args, **kwargs): ---------- Same as PlottingServer """ - host = kwargs["host"] if "host" in kwargs else None - port = kwargs["port"] if "port" in kwargs else None + host = kwargs["host"] if "host" in kwargs else _DEFAULT_HOST + port = _find_available_tcp_port(host) if "port" not in kwargs or kwargs["port"] is None else kwargs["port"] + + kwargs["host"] = host + kwargs["port"] = port + log_level = None if "log_level" in kwargs: log_level = kwargs["log_level"] diff --git a/bioptim/gui/online_callback_server.py b/bioptim/gui/online_callback_server.py index c51563a6c..a1138957c 100644 --- a/bioptim/gui/online_callback_server.py +++ b/bioptim/gui/online_callback_server.py @@ -17,6 +17,7 @@ from ..misc.parameters_types import ( Bool, Int, + Float, Str, Bytes, StrOptional, @@ -485,7 +486,8 @@ def __init__( self._host: Str = host if host else _DEFAULT_HOST self._port: Int = port if port else _DEFAULT_PORT - self._socket: socket.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._socket: socket.socket | None = None + self._reset_client_socket() self._should_wait_ok_to_client_on_new_data: Bool = platform.system() == "Darwin" @@ -502,6 +504,14 @@ def __init__( self._initialize_connexion(**show_options) + def _reset_client_socket(self) -> None: + if self._socket is not None: + try: + self._socket.close() + except OSError: + pass + self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + def _initialize_connexion(self, retries: Int = 0, **show_options) -> None: """ Initializes the connexion to the server @@ -522,10 +532,11 @@ def _initialize_connexion(self, retries: Int = 0, **show_options) -> None: if retries > 5: raise RuntimeError( "Could not connect to the plotter server, make sure it is running by calling 'PlottingServer()' on " - "another python instance or allowing for automatic start (Linux or Windows) of the server setting " + "another python instance or allowing for automatic start (Linux, Windows or macOS) of the server setting " "the online_option to 'OnlineOptim.MULTIPROCESS_SERVER' when instantiating your solver." ) else: + self._reset_client_socket() time.sleep(1) return self._initialize_connexion(retries + 1, **show_options) diff --git a/bioptim/gui/plot.py b/bioptim/gui/plot.py index fa7a88997..bfacd6fea 100644 --- a/bioptim/gui/plot.py +++ b/bioptim/gui/plot.py @@ -695,10 +695,15 @@ def _add_bounds_to_plot( self, i: Int, nlp: NonLinearProgram, variable: Str, ctr: Int, ax: plt.Axes, mapping_to_first_index: IntList ) -> None: """Add bounds to a specific plot""" - if nlp.plot[variable].bounds.type == InterpolationType.EACH_FRAME: + if nlp.plot[variable].bounds.type in [InterpolationType.EACH_FRAME, InterpolationType.ALL_POINTS]: ns = nlp.plot[variable].bounds.min.shape[1] - 1 else: ns = nlp.ns + t = ( + [np.linspace(0, ns, ns + 1) for i in range(len(self.t))] + if nlp.plot[variable].bounds.type == InterpolationType.ALL_POINTS + else self.t + ) # TODO: introduce repeat for the COLLOCATIONS min/max_bounds only for states graphs. # For now the plots in COLLOCATIONS with LINEAR are not giving the right values @@ -712,8 +717,8 @@ def _add_bounds_to_plot( bounds_min = np.concatenate((bounds_min, [bounds_min[-1]])) bounds_max = np.concatenate((bounds_max, [bounds_max[-1]])) - self.plots_bounds.append([ax.step(self.t[i], bounds_min, where="post", **self.plot_options["bounds"]), i]) - self.plots_bounds.append([ax.step(self.t[i], bounds_max, where="post", **self.plot_options["bounds"]), i]) + self.plots_bounds.append([ax.step(t[i], bounds_min, where="post", **self.plot_options["bounds"]), i]) + self.plots_bounds.append([ax.step(t[i], bounds_max, where="post", **self.plot_options["bounds"]), i]) def _add_new_axis(self, variable: Str, nb: Int, n_rows: Int, n_cols: Int) -> np.ndarray[plt.Axes]: """ @@ -1185,8 +1190,13 @@ def _update_ydata(self, ydata: DMList | NpArrayList) -> None: y_min = np.inf for p in ax.get_children(): if isinstance(p, lines.Line2D): - y_min = min(y_min, np.nanmin(p.get_ydata())) - y_max = max(y_max, np.nanmax(p.get_ydata())) + y_data = np.asarray(p.get_ydata()) + if y_data.size == 0 or np.isnan(y_data).all(): + continue + y_min = min(y_min, np.nanmin(y_data)) + y_max = max(y_max, np.nanmax(y_data)) + if not np.isfinite(y_min) or not np.isfinite(y_max): + continue ax.set_ylim(self._compute_ylim(y_min, y_max, 1.25)) for p in self.plots_vertical_lines: diff --git a/bioptim/interfaces/interface_utils.py b/bioptim/interfaces/interface_utils.py index 118848346..1a197101b 100644 --- a/bioptim/interfaces/interface_utils.py +++ b/bioptim/interfaces/interface_utils.py @@ -1,3 +1,4 @@ +import platform from time import perf_counter from casadi import Importer, Function, horzcat, vertcat, sum1, sum2, nlpsol, SX, MX, DM, reshape, jacobian @@ -37,7 +38,14 @@ def generic_online_optim(interface: SolverInterface, ocp, show_options: AnyDictO online_optim = interface.opts.online_optim.get_default() if online_optim is None: return - elif online_optim == OnlineOptim.MULTIPROCESS: + if platform.system() == "Darwin" and online_optim == OnlineOptim.MULTIPROCESS: + raise NotImplementedError( + "online_optim MULTIPROCESS is not available on macOS. " + "Use OnlineOptim.MULTIPROCESS_SERVER for automatic plotting or OnlineOptim.SERVER with a manually started " + "PlottingServer." + ) + + if online_optim == OnlineOptim.MULTIPROCESS: to_call = OnlineCallbackMultiprocess elif online_optim == OnlineOptim.SERVER: to_call = OnlineCallbackServer diff --git a/bioptim/limits/objective_functions.py b/bioptim/limits/objective_functions.py index 0d82f7962..0699ec999 100644 --- a/bioptim/limits/objective_functions.py +++ b/bioptim/limits/objective_functions.py @@ -416,6 +416,7 @@ def get_type() -> Callable TRACK_SEGMENT_WITH_CUSTOM_RT = (PenaltyFunctionAbstract.Functions.track_segment_with_custom_rt,) TRACK_SOFT_CONTACT_FORCES = (PenaltyFunctionAbstract.Functions.minimize_soft_contact_forces,) TRACK_STATE = (PenaltyFunctionAbstract.Functions.minimize_states,) + TRACK_ALGEBRAIC_STATES = (PenaltyFunctionAbstract.Functions.minimize_algebraic_states,) @staticmethod def get_type() -> Callable: @@ -455,6 +456,7 @@ def get_type() -> Callable MINIMIZE_TIME = (ObjectiveFunction.MayerFunction.Functions.minimize_time,) PROPORTIONAL_STATE = (PenaltyFunctionAbstract.Functions.proportional_states,) MINIMIZE_ALGEBRAIC_STATE = (PenaltyFunctionAbstract.Functions.minimize_algebraic_states,) + TRACK_ALGEBRAIC_STATE = (PenaltyFunctionAbstract.Functions.minimize_algebraic_states,) SUPERIMPOSE_MARKERS = (PenaltyFunctionAbstract.Functions.superimpose_markers,) SUPERIMPOSE_MARKERS_VELOCITY = (PenaltyFunctionAbstract.Functions.superimpose_markers_velocity,) TRACK_MARKER_WITH_SEGMENT_AXIS = (PenaltyFunctionAbstract.Functions.track_marker_with_segment_axis,) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index a1a8b7114..09caa8a0d 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -163,6 +163,12 @@ def minimize_algebraic_states(penalty: PenaltyOption, controller: PenaltyControl """ penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic + is_target_plotable = ( + penalty.integration_rule != QuadratureRule.APPROXIMATE_TRAPEZOIDAL + and penalty.integration_rule != QuadratureRule.TRAPEZOIDAL + ) + if is_target_plotable: + penalty.add_target_to_plot(controller=controller, combine_to=f"{key}") penalty.multi_thread = True if penalty.multi_thread is None else penalty.multi_thread return controller.algebraic_states[key].cx_start diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index 44b016fd8..79168df10 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -102,7 +102,7 @@ class OnlineOptim(Enum): Attributes ---------- NONE: No online plotting - DEFAULT: Default online plotting (MULTIPROCESS on Linux, MULTIPROCESS_SERVER on Windows and NONE on MacOS) + DEFAULT: Default online plotting (MULTIPROCESS on Linux and MULTIPROCESS_SERVER on Windows and macOS) MULTIPROCESS: Multiprocess online plotting SERVER: Server online plotting MULTIPROCESS_SERVER: Multiprocess server online plotting @@ -117,9 +117,7 @@ def get_default(self): if self != OnlineOptim.DEFAULT: return self - if platform.system() == "Linux": - return OnlineOptim.MULTIPROCESS - elif platform.system() == "Windows": + if platform.system() in ("Linux", "Windows", "Darwin"): return OnlineOptim.MULTIPROCESS_SERVER else: return None diff --git a/bioptim/misc/mapping.py b/bioptim/misc/mapping.py index 732110fdb..8d108c4d6 100644 --- a/bioptim/misc/mapping.py +++ b/bioptim/misc/mapping.py @@ -408,11 +408,12 @@ def __init__( dependency_matrix: list = [None for _ in range(len(first))] oppose = [] for i in range(len(first)): - if first[i] != 0 and first[i] > 0: - dependency_matrix[i] = int(first[i] - 1) - if first[i] < 0: + value = first[i, 0] + if value != 0 and value > 0: + dependency_matrix[i] = int(value - 1) + if value < 0: oppose.append(i) - dependency_matrix[i] = int(abs(first[i]) - 1) + dependency_matrix[i] = int(abs(value) - 1) def _build_to_second(dependency_matrix: AnyList, independent_indices: AnyTuple): """ diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index ed7b259e1..299604eba 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -198,7 +198,6 @@ def set_gravity(self, new_gravity: Parameter | MX | np.ndarray) -> None: self.model.setGravity(new_gravity.mx) else: self.model.setGravity(new_gravity) - return @property def nb_tau(self) -> int: @@ -537,7 +536,7 @@ def forward_dynamics_free_floating_base(self) -> Function: return casadi_fun @staticmethod - def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: + def reorder_qddot_root_joints(qddot_root: CX, qddot_joints: CX) -> CX: return vertcat(qddot_root, qddot_joints) def _dispatch_forces(self) -> biorbd.ExternalForceSet: diff --git a/bioptim/models/biorbd/model_dynamics.py b/bioptim/models/biorbd/model_dynamics.py index 6a05c245f..8d5ee8c7f 100644 --- a/bioptim/models/biorbd/model_dynamics.py +++ b/bioptim/models/biorbd/model_dynamics.py @@ -1,11 +1,7 @@ from typing import Callable import biorbd_casadi as biorbd -import numpy as np -from .external_forces import ( - ExternalForceSetTimeSeries, - ExternalForceSetVariables, -) +from .external_forces import ExternalForceSetTimeSeries, ExternalForceSetVariables from ...optimization.parameters import ParameterList from .biorbd_model import BiorbdModel from .multi_biorbd_model import MultiBiorbdModel @@ -26,13 +22,7 @@ MusclesDynamicsWithExcitations, ) from ..protocols.holonomic_constraints import HolonomicConstraintsList -from ...misc.parameters_types import ( - Str, - Int, - Bool, - NpArray, - DM, -) +from ...misc.parameters_types import Str, Int, Bool, NpArray, DM from ...misc.mapping import BiMappingList from ...misc.enums import ContactType, QuadratureRule, ControlType from ...optimization.problem_type import SocpType diff --git a/bioptim/models/pinocchio/__init__.py b/bioptim/models/pinocchio/__init__.py new file mode 100644 index 000000000..910a1856d --- /dev/null +++ b/bioptim/models/pinocchio/__init__.py @@ -0,0 +1,2 @@ +from .pinocchio_model import PinocchioModel +from .model_dynamics import TorquePinocchioModel diff --git a/bioptim/models/pinocchio/model_dynamics.py b/bioptim/models/pinocchio/model_dynamics.py new file mode 100644 index 000000000..71395f63b --- /dev/null +++ b/bioptim/models/pinocchio/model_dynamics.py @@ -0,0 +1,7 @@ +from .pinocchio_model import PinocchioModel +from ...dynamics.state_space_dynamics import TorqueDynamics + + +class TorquePinocchioModel(PinocchioModel, TorqueDynamics): + def __init__(self, bio_model: str | object, **kwargs): + super().__init__(bio_model=bio_model, **kwargs) diff --git a/bioptim/models/pinocchio/pinocchio_model.py b/bioptim/models/pinocchio/pinocchio_model.py new file mode 100644 index 000000000..5291e134d --- /dev/null +++ b/bioptim/models/pinocchio/pinocchio_model.py @@ -0,0 +1,564 @@ +from typing import Callable + +from casadi import SX, Function, horzcat, vertcat +import numpy as np + +from ..utils import _var_mapping, bounds_from_ranges, cache_function, Range +from ...limits.path_conditions import Bounds +from ...misc.enums import ContactType +from ...misc.mapping import BiMapping, BiMappingList +from ...optimization.parameters import Parameter, ParameterList + + +def _import_pinocchio(): + try: + import pinocchio as pin + import pinocchio.casadi as cpin + except ModuleNotFoundError as e: + raise ModuleNotFoundError( + "PinocchioModel requires the optional dependency 'pinocchio'. " + "Install the Pinocchio Python bindings to use this model backend." + ) from e + + return pin, cpin + + +class PinocchioModel: + """ + Pinocchio implementation of the bioptim biomodel protocol. + + The implementation intentionally mirrors the BiorbdModel public surface where Pinocchio exposes equivalent + rigid-body algorithms. Muscle, ligament, passive torque, rigid contact and biorbd external-force APIs are not + available yet. + """ + + def __init__( + self, + bio_model: str | object, + parameters: ParameterList = None, + **kwargs, + ): + super().__init__(**kwargs) # For multiple inheritance compatibility + self._pinocchio_module, self._casadi_pinocchio_module = _import_pinocchio() + self._path = None + + if isinstance(bio_model, str): + self._path = bio_model + self._model = self._pinocchio_module.buildModelFromUrdf(bio_model) + elif isinstance(bio_model, self._pinocchio_module.Model): + self._model = bio_model + else: + raise ValueError("The model should be of type 'str' or 'pinocchio.Model'") + + self._casadi_model = self._casadi_pinocchio_module.Model(self._model) + self._data = self._casadi_model.createData() + + if parameters is not None: + for param_key in parameters: + parameters[param_key].apply_parameter(self) + self.parameters = parameters.sx if parameters else SX() + + self._marker_names = self._extract_marker_names() + self._symbolic_variables() + self._cached_functions = {} + + def _symbolic_variables(self): + self.q = SX.sym("q_sx", self.nb_q, 1) + self.qdot = SX.sym("qdot_sx", self.nb_qdot, 1) + self.qddot = SX.sym("qddot_sx", self.nb_qddot, 1) + self.qddot_joints = SX.sym("qddot_joints_sx", self.nb_qddot - self.nb_root, 1) + self.tau = SX.sym("tau_sx", self.nb_tau, 1) + self.external_forces = SX.sym("external_forces_sx", 0, 1) + self.muscle = SX.sym("muscle_sx", 0, 1) + self.activations = SX.sym("activations_sx", 0, 1) + + def _extract_marker_names(self) -> tuple[str, ...]: + return tuple( + frame.name for frame in self._model.frames[1:] if frame.type == self._pinocchio_module.FrameType.FIXED_JOINT + ) + + @property + def name(self) -> str: + return self._model.name + + @property + def path(self) -> str | None: + return self._path + + def copy(self): + if self.path: + return PinocchioModel(self.path) + return PinocchioModel(self._model) + + def serialize(self) -> tuple[Callable, dict]: + bio_model = self.path if self.path else self._model + return PinocchioModel, dict(bio_model=bio_model) + + @cache_function + def gravity(self) -> Function: + gravity = SX(self._model.gravity.linear) + return Function("gravity", [self.parameters], [gravity], ["parameters"], ["gravity"]) + + def set_gravity(self, new_gravity: Parameter | SX | np.ndarray) -> None: + new_value = new_gravity.sx if isinstance(new_gravity, Parameter) else new_gravity + self._model.gravity.linear = np.asarray(new_value, dtype=float).reshape(3) + + @property + def nb_tau(self) -> int: + return self._model.nv + + @property + def nb_segments(self) -> int: + return self._model.njoints - 1 + + def segment_index(self, name) -> int: + if name not in self._model.names: + raise ValueError(f"{name} is not a segment name") + return list(self._model.names).index(name) + + @property + def nb_quaternions(self) -> int: + return max(self.nb_q - self.nb_qdot, 0) + + @property + def nb_dof(self) -> int: + return self._model.nv + + @property + def name_dofs(self) -> tuple[str, ...]: + names = [] + for joint_id in range(1, self._model.njoints): + joint_name = self._model.names[joint_id] + joint_nv = self._model.joints[joint_id].nv + names.extend([joint_name] if joint_nv == 1 else [f"{joint_name}_{i}" for i in range(joint_nv)]) + return tuple(names) + + @property + def nb_q(self) -> int: + return self._model.nq + + @property + def nb_qdot(self) -> int: + return self._model.nv + + @property + def nb_qddot(self) -> int: + return self._model.nv + + @property + def nb_root(self) -> int: + if self._model.njoints <= 1: + return 0 + first_joint = self._model.joints[1] + return first_joint.nv if first_joint.nq == 7 and first_joint.nv == 6 else 0 + + @property + def segments(self) -> tuple: + return tuple(self._model.joints[1:]) + + def _frame_placement(self, frame_index: int, q: SX = None): + self._casadi_pinocchio_module.framesForwardKinematics( + self._casadi_model, self._data, self.q if q is None else q + ) + return self._data.oMf[frame_index] + + def _frame_translation(self, frame_index: int, q: SX = None) -> SX: + return self._frame_placement(frame_index, q).translation + + def _frame_homogeneous_matrix(self, frame_index: int, q: SX = None) -> SX: + placement = self._frame_placement(frame_index, q) + out = SX.eye(4) + out[:3, :3] = placement.rotation + out[:3, 3] = placement.translation + return out + + @cache_function + def homogeneous_matrices_in_global(self, segment_index: int, inverse: bool = False) -> Function: + matrix = self._frame_homogeneous_matrix(segment_index) + if inverse: + rotation = matrix[:3, :3] + translation = matrix[:3, 3] + matrix = SX.eye(4) + matrix[:3, :3] = rotation.T + matrix[:3, 3] = -rotation.T @ translation + + return Function( + "homogeneous_matrices_in_global", + [self.q, self.parameters], + [matrix], + ["q", "parameters"], + ["Joint coordinate system RT matrix in global"], + ) + + @cache_function + def homogeneous_matrices_in_child(self, segment_id) -> Function: + placement = self._casadi_model.frames[segment_id].placement + matrix = SX.eye(4) + matrix[:3, :3] = placement.rotation + matrix[:3, 3] = placement.translation + return Function( + "homogeneous_matrices_in_child", + [self.parameters], + [matrix], + ["parameters"], + ["Joint coordinate system RT matrix in local"], + ) + + @cache_function + def mass(self) -> Function: + model_mass = sum(inertia.mass for inertia in self._model.inertias) + return Function("mass", [self.parameters], [SX(model_mass)], ["parameters"], ["mass"]) + + @cache_function + def rt(self, rt_index) -> Function: + return self.homogeneous_matrices_in_global(rt_index) + + @cache_function + def center_of_mass(self) -> Function: + com = self._casadi_pinocchio_module.centerOfMass(self._casadi_model, self._data, self.q) + return Function("center_of_mass", [self.q, self.parameters], [com], ["q", "parameters"], ["Center of mass"]) + + @cache_function + def center_of_mass_velocity(self) -> Function: + jacobian = self._casadi_pinocchio_module.jacobianCenterOfMass(self._casadi_model, self._data, self.q) + com_velocity = jacobian @ self.qdot + return Function( + "center_of_mass_velocity", + [self.q, self.qdot, self.parameters], + [com_velocity], + ["q", "qdot", "parameters"], + ["Center of mass velocity"], + ) + + @cache_function + def center_of_mass_acceleration(self) -> Function: + self._casadi_pinocchio_module.centerOfMass(self._casadi_model, self._data, self.q, self.qdot, self.qddot) + com_acceleration = self._data.acom[0] + return Function( + "center_of_mass_acceleration", + [self.q, self.qdot, self.qddot, self.parameters], + [com_acceleration], + ["q", "qdot", "qddot", "parameters"], + ["Center of mass acceleration"], + ) + + @cache_function + def mass_matrix(self) -> Function: + mass_matrix = self._casadi_pinocchio_module.crba(self._casadi_model, self._data, self.q) + return Function("mass_matrix", [self.q, self.parameters], [mass_matrix], ["q", "parameters"], ["Mass matrix"]) + + @cache_function + def non_linear_effects(self) -> Function: + effects = self._casadi_pinocchio_module.nonLinearEffects(self._casadi_model, self._data, self.q, self.qdot) + return Function( + "non_linear_effects", + [self.q, self.qdot, self.parameters], + [effects], + ["q", "qdot", "parameters"], + ["Non linear effects"], + ) + + @cache_function + def angular_momentum(self) -> Function: + self._casadi_pinocchio_module.computeCentroidalMomentum(self._casadi_model, self._data, self.q, self.qdot) + return Function( + "angular_momentum", + [self.q, self.qdot, self.parameters], + [self._data.hg.angular], + ["q", "qdot", "parameters"], + ["Angular momentum"], + ) + + @cache_function + def reshape_qdot(self, k_stab=1) -> Function: + return Function( + "reshape_qdot", + [self.q, self.qdot, self.parameters], + [self.qdot], + ["q", "qdot", "parameters"], + ["Reshaped qdot"], + ) + + @cache_function + def segment_angular_velocity(self, idx) -> Function: + self._casadi_pinocchio_module.forwardKinematics(self._casadi_model, self._data, self.q, self.qdot) + velocity = self._casadi_pinocchio_module.getVelocity( + self._casadi_model, self._data, idx, self._casadi_pinocchio_module.ReferenceFrame.LOCAL_WORLD_ALIGNED + ) + return Function( + "segment_angular_velocity", + [self.q, self.qdot, self.parameters], + [velocity.angular], + ["q", "qdot", "parameters"], + ["Segment angular velocity"], + ) + + @property + def friction_coefficients(self) -> SX | np.ndarray: + return None + + @property + def nb_passive_joint_torques(self) -> int: + return 0 + + @property + def contact_types(self) -> list[ContactType]: + return [] + + @property + def nb_soft_contacts(self) -> int: + return 0 + + @property + def nb_muscles(self) -> int: + return 0 + + @property + def nb_ligaments(self) -> int: + return 0 + + @staticmethod + def reorder_qddot_root_joints(qddot_root: SX, qddot_joints: SX) -> SX: + return vertcat(qddot_root, qddot_joints) + + @cache_function + def forward_dynamics(self, with_contact: bool = False) -> Function: + if with_contact: + raise NotImplementedError("forward_dynamics with contact is not implemented yet for PinocchioModel") + + qddot = self._casadi_pinocchio_module.aba(self._casadi_model, self._data, self.q, self.qdot, self.tau) + return Function( + "forward_dynamics", + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], + [qddot], + ["q", "qdot", "tau", "external_forces", "parameters"], + ["qddot"], + ) + + @cache_function + def inverse_dynamics(self, with_contact: bool = False) -> Function: + if with_contact: + raise NotImplementedError("inverse_dynamics with contact is not implemented yet for PinocchioModel") + + tau = self._casadi_pinocchio_module.rnea(self._casadi_model, self._data, self.q, self.qdot, self.qddot) + return Function( + "inverse_dynamics", + [self.q, self.qdot, self.qddot, self.external_forces, self.parameters], + [tau], + ["q", "qdot", "qddot", "external_forces", "parameters"], + ["tau"], + ) + + @cache_function + def forward_dynamics_derivatives(self) -> Function: + self._casadi_pinocchio_module.computeABADerivatives(self._casadi_model, self._data, self.q, self.qdot, self.tau) + return Function( + "forward_dynamics_derivatives", + [self.q, self.qdot, self.tau, self.parameters], + [self._data.ddq_dq, self._data.ddq_dv, self._data.Minv], + ["q", "qdot", "tau", "parameters"], + ["ddq_dq", "ddq_dqdot", "ddq_dtau"], + ) + + @cache_function + def inverse_dynamics_derivatives(self) -> Function: + derivatives = self._casadi_pinocchio_module.computeRNEADerivatives( + self._casadi_model, self._data, self.q, self.qdot, self.qddot + ) + if derivatives is not None: + dtau_dq, dtau_dv, dtau_da = derivatives + else: + dtau_dq, dtau_dv, dtau_da = self._data.dtau_dq, self._data.dtau_dv, self._data.M + return Function( + "inverse_dynamics_derivatives", + [self.q, self.qdot, self.qddot, self.parameters], + [dtau_dq, dtau_dv, dtau_da], + ["q", "qdot", "qddot", "parameters"], + ["dtau_dq", "dtau_dqdot", "dtau_dqddot"], + ) + + @property + def marker_names(self) -> tuple[str, ...]: + return self._marker_names + + @property + def nb_markers(self) -> int: + return len(self.marker_names) + + def marker_index(self, name): + if name not in self.marker_names: + raise ValueError(f"{name} is not a marker name") + return self.marker_names.index(name) + + def _marker_frame_index(self, index: int) -> int: + return self._model.getFrameId(self.marker_names[index]) + + @cache_function + def markers(self) -> Function: + markers = horzcat(*[self._frame_translation(self._marker_frame_index(i)) for i in range(self.nb_markers)]) + return Function("markers", [self.q, self.parameters], [markers], ["q", "parameters"], ["markers"]) + + @cache_function + def marker(self, index: int, reference_segment_index: int = None) -> Function: + position = self._frame_translation(self._marker_frame_index(index)) + if reference_segment_index is not None: + reference_matrix = self._frame_homogeneous_matrix(reference_segment_index) + position = reference_matrix[:3, :3].T @ (position - reference_matrix[:3, 3]) + return Function("marker", [self.q, self.parameters], [position], ["q", "parameters"], ["marker"]) + + @property + def nb_rigid_contacts(self) -> int: + return 0 + + @property + def nb_contacts(self) -> int: + return 0 + + @cache_function + def markers_velocities(self, reference_index=None) -> Function: + velocities = horzcat( + *[self.marker_velocity(i)(self.q, self.qdot, self.parameters) for i in range(self.nb_markers)] + ) + return Function( + "markers_velocities", + [self.q, self.qdot, self.parameters], + [velocities], + ["q", "qdot", "parameters"], + ["markers_velocities"], + ) + + @cache_function + def marker_velocity(self, marker_index: int) -> Function: + self._casadi_pinocchio_module.forwardKinematics(self._casadi_model, self._data, self.q, self.qdot) + self._casadi_pinocchio_module.updateFramePlacements(self._casadi_model, self._data) + velocity = self._casadi_pinocchio_module.getFrameVelocity( + self._casadi_model, + self._data, + self._marker_frame_index(marker_index), + self._casadi_pinocchio_module.ReferenceFrame.LOCAL_WORLD_ALIGNED, + ).linear + return Function( + "marker_velocity", + [self.q, self.qdot, self.parameters], + [velocity], + ["q", "qdot", "parameters"], + ["marker_velocity"], + ) + + @cache_function + def markers_accelerations(self, reference_index=None) -> Function: + accelerations = horzcat( + *[ + self.marker_acceleration(i)(self.q, self.qdot, self.qddot, self.parameters) + for i in range(self.nb_markers) + ] + ) + return Function( + "markers_accelerations", + [self.q, self.qdot, self.qddot, self.parameters], + [accelerations], + ["q", "qdot", "qddot", "parameters"], + ["markers_accelerations"], + ) + + @cache_function + def marker_acceleration(self, marker_index: int) -> Function: + self._casadi_pinocchio_module.forwardKinematics(self._casadi_model, self._data, self.q, self.qdot, self.qddot) + self._casadi_pinocchio_module.updateFramePlacements(self._casadi_model, self._data) + acceleration = self._casadi_pinocchio_module.getFrameAcceleration( + self._casadi_model, + self._data, + self._marker_frame_index(marker_index), + self._casadi_pinocchio_module.ReferenceFrame.LOCAL_WORLD_ALIGNED, + ).linear + return Function( + "marker_acceleration", + [self.q, self.qdot, self.qddot, self.parameters], + [acceleration], + ["q", "qdot", "qddot", "parameters"], + ["marker_acceleration"], + ) + + @cache_function + def tau_max(self) -> Function: + return Function( + "tau_max", + [self.q, self.qdot, self.parameters], + [SX(self._model.effortLimit), -SX(self._model.effortLimit)], + ["q", "qdot", "parameters"], + ["tau_max", "tau_min"], + ) + + @cache_function + def markers_jacobian(self) -> Function: + jacobians = [] + for marker_index in range(self.nb_markers): + try: + jacobian = self._casadi_pinocchio_module.computeFrameJacobian( + self._casadi_model, + self._data, + self.q, + self._marker_frame_index(marker_index), + self._casadi_pinocchio_module.ReferenceFrame.LOCAL_WORLD_ALIGNED, + ) + except TypeError: + jacobian = SX.zeros(6, self.nb_qdot) + self._casadi_pinocchio_module.computeFrameJacobian( + self._casadi_model, + self._data, + self.q, + self._marker_frame_index(marker_index), + self._casadi_pinocchio_module.ReferenceFrame.LOCAL_WORLD_ALIGNED, + jacobian, + ) + jacobians.append(jacobian[:3, :]) + return Function( + "markers_jacobian", + [self.q, self.parameters], + jacobians, + ["q", "parameters"], + ["markers_jacobian"], + ) + + def ranges_from_model(self, variable: str): + if variable in ("q", "q_roots", "q_joints"): + lower = self._model.lowerPositionLimit + upper = self._model.upperPositionLimit + elif variable in ("qdot", "qdot_roots", "qdot_joints"): + lower = -self._model.velocityLimit + upper = self._model.velocityLimit + elif variable in ("qddot", "qddot_joints"): + lower = np.full(self.nb_qddot, -np.inf) + upper = np.full(self.nb_qddot, np.inf) + else: + raise RuntimeError("Wrong variable name") + + if "_joints" in variable and self.nb_root: + lower = lower[self.nb_root :] + upper = upper[self.nb_root :] + elif "_roots" in variable: + lower = lower[: self.nb_root] + upper = upper[: self.nb_root] + + return [Range(float(min_bound), float(max_bound)) for min_bound, max_bound in zip(lower, upper)] + + def _var_mapping( + self, + key: str, + range_for_mapping: int | list | tuple | range, + mapping: BiMapping = None, + ) -> dict: + return _var_mapping(key, range_for_mapping, mapping) + + def bounds_from_ranges(self, variables: str | list[str], mapping: BiMapping | BiMappingList = None) -> Bounds: + return bounds_from_ranges(self, variables, mapping) + + @cache_function + def lagrangian(self) -> Function: + kinetic = self._casadi_pinocchio_module.computeKineticEnergy(self._casadi_model, self._data, self.q, self.qdot) + potential = self._casadi_pinocchio_module.computePotentialEnergy(self._casadi_model, self._data, self.q) + return Function("lagrangian", [self.q, self.qdot], [kinetic - potential], ["q", "qdot"], ["lagrangian"]) + + @staticmethod + def animate(*args, **kwargs): + raise NotImplementedError("Animation is not implemented for PinocchioModel") diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index 25622e39c..26751104d 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -16,6 +16,7 @@ Bool, NpArrayListOptional, AnyListOptional, + CX, ) if TYPE_CHECKING: @@ -201,7 +202,7 @@ def forward_dynamics_free_floating_base(self) -> Function: args: q, qdot, qddot_joints """ - def reorder_qddot_root_joints(self) -> Function: + def reorder_qddot_root_joints(self, qddot_root: CX, qddot_joints: CX) -> CX: """ reorder the qddot, from the root dof and the joints dof args: qddot_root, qddot_joints diff --git a/bioptim/models/protocols/holonomic_constraints.py b/bioptim/models/protocols/holonomic_constraints.py index 7195a0d11..83ecbde09 100644 --- a/bioptim/models/protocols/holonomic_constraints.py +++ b/bioptim/models/protocols/holonomic_constraints.py @@ -4,7 +4,7 @@ from typing import Any, Callable -from casadi import MX, Function, jacobian, vertcat, trace, sqrt, DM +from casadi import MX, Function, acos, fmax, fmin, if_else, jacobian, sin, sqrt, trace, vertcat from .biomodel import BioModel from ...misc.options import OptionDict @@ -159,12 +159,11 @@ def superimpose_markers( return constraints_func, constraints_jacobian_func, bias_func @staticmethod - def align_frames( + def align_frames_small_angles( 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. @@ -193,7 +192,8 @@ def align_frames( This formulation: - Uses exactly 3 scalar equations (minimal representation) - - Avoids singularities at θ = 0 through Taylor expansion + - Avoids singularities at θ = 0 through Taylor expansion of order 6 of theta/sin(theta) + and theta using the first-order expansion of arccos such that arcos(3- trace(R)) ~ sqrt(3- trace(R)). - Is equivalent to the axis-angle formulation: Φ = (θ/sin(θ)) · ω̂ Parameters @@ -208,10 +208,6 @@ def align_frames( 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 ------- @@ -250,9 +246,11 @@ def align_frames( ----- The Taylor expansion used for θ/sin(θ) provides numerical stability near θ = 0: θ/sin(θ) ≈ 1 + θ²/6 + 7θ⁴/360 + 31θ⁶/15120 + The Taylor expansion used for θ = acos(...) provides numerical stability near θ = 0: + θ ≈ √ (3 - trace(R_rel)) + additionnally we guarantee that 3 - trace(R_rel) is positive through fmax and add a small ɛ̝ = 1e-12 - This ensures the constraint remains well-conditioned even when the frames are - nearly aligned. + This ensures the constraint remains well-conditioned even when the frames are nearly aligned. See Also -------- @@ -262,22 +260,14 @@ def align_frames( References ---------- .. [1] Murray, R. M., Li, Z., & Sastry, S. S. (1994). A Mathematical Introduction - to Robotic Manipulation. CRC Press. + 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 a local reference frame is requested, first bring the two frames into that local frame 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] @@ -294,30 +284,213 @@ def align_frames( 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 + # Relative rotation: R_rel = R2ᵀ·R1 (frame‑1 → frame‑2) + R_rel = R2.T @ R1 # still a symbolic 3×3 matrix + + theta = sqrt(fmax(3 - trace(R_rel), 0) + 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 6th order Taylor expansion + S = theta_over_sintheta * (R_rel - R_rel.T) / 2.0 + + constraint = vertcat(S[2, 1], -S[2, 0], S[1, 0]) # r32 - r23 # r13 - r31 # r21 - r12 + + 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 + + @staticmethod + def align_frames_orientation( + model, + frame_1_idx: int = 0, + frame_2_idx: int = 1, + local_frame_idx: int | None = None, + ) -> 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 of order 6 of theta/sin(theta) + and theta using the first-order expansion of arccos such that arcos(3- trace(R)) ~ sqrt(3- trace(R)). + - 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. + + 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 + The Taylor expansion used for θ = acos(...) provides numerical stability near θ = 0: + θ ≈ √ (3 - trace(R_rel)) + additionnally we guarantee that 3 - trace(R_rel) is positive through fmax and add a small ɛ̝ = 1e-12 + + This ensures the constraint remains well-conditioned even when the frames are nearly aligned. + + This function implements a smooth transition between the normal formulation and the small angle formulation. + This requires switching between different cases with if_else to avoid evaluationg functions where they are not defined. + + 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 + parameters = model.parameters # optional model parameters + + # If a local reference frame is requested, first bring the two frames into that local frame + 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 = R2ᵀ·R1 (frame‑1 → frame‑2) + R_rel = R2.T @ R1 # still a symbolic 3×3 matrix + + cos_theta = (trace(R_rel) - 1) / 2 + + # Clip to the admissible interval to avoid acos‑NaN caused by rounding + cos_theta_clip = fmax(fmin(cos_theta, 1.0), -1.0) + theta = acos(cos_theta_clip) # 0 ≤ θ ≤ π + + # Scaling factor = θ / (2·sinθ) – robust for θ≈0 and θ≈π + smooth_transition = 1e-1 # tolerance for singular limits + transition_epsilon = 2e-2 + hard_switch = (smooth_transition - transition_epsilon) / 2 + is_small = theta < hard_switch + + # regular case + sin_theta = sin(theta) + factor_regular = theta / sin_theta # normal case + + # small theta case + theta_small = sqrt(fmax(3 - trace(R_rel), 0) + 1e-12) # using the first-order expansion of arccos + theta_over_sintheta_taylor = ( + 1 + theta_small**2 / 6 + 7 * theta_small**4 / 360 + 31 * theta_small**6 / 15120 ) # using the Taylor expansion - S = theta_over_sintheta * (R_error - R_error.T) / 2.0 # still 3×3, skew‑symmetric + + t = fmin( + fmax(0, (theta - (smooth_transition - transition_epsilon / 2)) / transition_epsilon), 1 + ) # Shift and scale + s = t * t * (3 - 2 * t) # Smoothstep in [0,1] + factor_smooth = (1 - s) * theta_over_sintheta_taylor + s * factor_regular + + factor = if_else(is_small, theta_over_sintheta_taylor, factor_smooth) + + S = factor * (R_rel - R_rel.T) / 2.0 # 3×3 MX (or dummy for π) 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( @@ -348,6 +521,7 @@ def align_frames( return constraints_func, constraints_jacobian_func, bias_func + @staticmethod def rigid_contacts( model: BioModel = None, ) -> tuple[Function, Function, Function]: diff --git a/bioptim/models/utils.py b/bioptim/models/utils.py index 267ab6396..e3264208c 100644 --- a/bioptim/models/utils.py +++ b/bioptim/models/utils.py @@ -1,4 +1,6 @@ +from dataclasses import dataclass from functools import wraps + from ..limits.path_conditions import Bounds from ..misc.mapping import BiMapping, BiMappingList from ..misc.enums import ContactType @@ -45,6 +47,18 @@ def _var_mapping(key, range_for_mapping, mapping: BiMapping = None) -> dict: return mapping +@dataclass(frozen=True) +class Range: + min_bound: float + max_bound: float + + def min(self) -> float: + return self.min_bound + + def max(self) -> float: + return self.max_bound + + def bounds_from_ranges(model, key: str, mapping: BiMapping | BiMappingList = None) -> Bounds: """ Generate bounds from the ranges of the model diff --git a/bioptim/optimization/receding_horizon_optimization.py b/bioptim/optimization/receding_horizon_optimization.py index d1c064e8a..a0dd3e4af 100644 --- a/bioptim/optimization/receding_horizon_optimization.py +++ b/bioptim/optimization/receding_horizon_optimization.py @@ -775,8 +775,8 @@ def solve( ) if self.parameters.shape != 0 and get_all_iterations: - final_solution_parameters_dict = [{key: None} for key in solution[0].parameters.keys()][0] - for key in solution[0].parameters.keys(): + final_solution_parameters_dict = [{key: None} for key in solution[1][0].parameters.keys()][0] + for key in solution[1][0].parameters.keys(): key_val = [] for sol in solution[1]: key_val.append(sol.parameters[key]) diff --git a/bioptim/optimization/solution/solution.py b/bioptim/optimization/solution/solution.py index 87446a945..cbeea6273 100644 --- a/bioptim/optimization/solution/solution.py +++ b/bioptim/optimization/solution/solution.py @@ -277,12 +277,11 @@ def from_initial_guess(cls, ocp: "OptimalControlProgram", sol: AnyList): dt, sol_states, sol_controls, sol_params, sol_algebraic_states = sol - vector = np.ndarray((0, 1)) - # For time if len(dt.shape) == 1: dt = dt[:, np.newaxis] - vector = np.concatenate((vector, dt)) + + state_vector = [dt] # For states for p, ss in enumerate(sol_states): @@ -299,9 +298,8 @@ def from_initial_guess(cls, ocp: "OptimalControlProgram", sol: AnyList): for i in range(all_ns[p] * nb_intermediate_frames + 1): for key in ss.keys(): - vector = np.concatenate( - (vector, ss[key].init.evaluate_at(i, nb_intermediate_frames)[:, np.newaxis]) - ) + state_vector.append(ss[key].init.evaluate_at(i, nb_intermediate_frames)[:, None]) + vector = np.vstack(state_vector) # For controls for p, ss in enumerate(sol_controls): diff --git a/docs/contributing.md b/docs/contributing.md index 6bc7b6829..b1636d7be 100644 --- a/docs/contributing.md +++ b/docs/contributing.md @@ -76,7 +76,7 @@ I won't get into details here, if you haven't yet, you should read them :) All variable names that could be plural should be written as such. -Black version 25.11 is used to enforce the code linting. +Black version black-26.5 is used to enforce the code linting. `Bioptim` is linted with the 120-character max per line's option. This means that your pull-request tests on GitHub will appear to fail if black fails. The easiest way to make sure black is happy is to locally run this command: diff --git a/docs/pinocchio_model.md b/docs/pinocchio_model.md new file mode 100644 index 000000000..c7d394ca1 --- /dev/null +++ b/docs/pinocchio_model.md @@ -0,0 +1,51 @@ +# Pinocchio model backend + +`PinocchioModel` is an optional backend that mirrors the rigid-body subset of `BiorbdModel`. +It loads URDF models with Pinocchio and wraps Pinocchio CasADi algorithms in the `Function` signatures expected by +bioptim. + +## Current equivalent functions + +| bioptim / biorbd surface | Pinocchio equivalent used | +| --- | --- | +| `forward_dynamics()` | `pinocchio.casadi.aba` | +| `inverse_dynamics()` | `pinocchio.casadi.rnea` | +| `mass_matrix()` | `pinocchio.casadi.crba` | +| `non_linear_effects()` | `pinocchio.casadi.nonLinearEffects` | +| `center_of_mass()` | `pinocchio.casadi.centerOfMass` | +| `center_of_mass_velocity()` | `pinocchio.casadi.jacobianCenterOfMass(q) @ qdot` | +| `center_of_mass_acceleration()` | `pinocchio.casadi.centerOfMass(q, qdot, qddot)` and `data.acom[0]` | +| `homogeneous_matrices_in_global()` | `pinocchio.casadi.framesForwardKinematics` and `data.oMf[frame_id]` | +| `marker()` / `markers()` | Pinocchio frames selected through `marker_names` | +| `marker_velocity()` | `pinocchio.casadi.forwardKinematics` and `getFrameVelocity` | +| `marker_acceleration()` | second-order `pinocchio.casadi.forwardKinematics` and `getFrameAcceleration` | +| `markers_jacobian()` | `pinocchio.casadi.computeFrameJacobian` | + +The unavailable biorbd-specific surfaces currently raise `NotImplementedError`: muscles, ligaments, passive torques, +soft contacts, rigid contact dynamics, biorbd external forces, animation and model parameter callbacks. + +## Analytical derivatives + +Pinocchio exposes analytical derivatives for the rigid-body algorithms that dominate torque-driven dynamics: + +| Quantity | Pinocchio function | Data returned by `PinocchioModel` | +| --- | --- | --- | +| `d aba / d(q, qdot, tau)` | `pinocchio.casadi.computeABADerivatives` | `ddq_dq`, `ddq_dqdot`, `ddq_dtau` | +| `d rnea / d(q, qdot, qddot)` | `pinocchio.casadi.computeRNEADerivatives` | `dtau_dq`, `dtau_dqdot`, `dtau_dqddot` | + +These are exposed as CasADi `Function`s through `forward_dynamics_derivatives()` and +`inverse_dynamics_derivatives()`. They are not yet wired into bioptim's NLP graph construction, which still relies on +CasADi differentiation of the assembled symbolic graph. The next optimization step would be to add an optional derivative +provider interface in the dynamics layer so solvers can use these functions when the selected backend offers them. + +On the biorbd side, the current bioptim integration already benefits from CasADi algorithmic differentiation through +`biorbd_casadi`, but this codebase does not expose a comparable first-class analytical ABA/RNEA derivative API to call +from `BiorbdModel`. + +References used while mapping the API: + +- Pinocchio automatic differentiation documentation: https://docs.ros.org/en/rolling/p/pinocchio/doc/a-features/k-automatic-differentiation.html +- Pinocchio frame Jacobian documentation: https://docs.ros.org/en/melodic/api/pinocchio/html/namespacepinocchio.html +- Pinocchio ABA derivatives API: https://docs.ros.org/en/ros2_packages/rolling/api/pinocchio/generated/file_include_pinocchio_algorithm_aba-derivatives.hxx.html +- Pinocchio RNEA derivatives API: https://docs.ros.org/en/rolling/p/pinocchio/generated/file_include_pinocchio_algorithm_rnea-derivatives.hxx.html + diff --git a/environment.yml b/environment.yml index 862d0de8f..4636177dd 100644 --- a/environment.yml +++ b/environment.yml @@ -4,9 +4,10 @@ channels: - conda-forge dependencies: - python >=3.10 -- biorbd >=1.12.0 +- biorbd >=1.12,<1.13 - matplotlib - pyqt - pyqtgraph - python-graphviz -- numpy <2.4 \ No newline at end of file +- numpy >=2.4,<3 +- pinocchio diff --git a/pyproject.toml b/pyproject.toml index fb515625d..925f20264 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,6 +24,7 @@ dependencies = [] [project.optional-dependencies] conda_only = [ "biorbd >=1.12.0", + "pinocchio", "pyqt", "pyqtgraph", "python-graphviz", diff --git a/resources/plotting_server.py b/resources/plotting_server.py index fcc103e3c..14ace7150 100644 --- a/resources/plotting_server.py +++ b/resources/plotting_server.py @@ -7,8 +7,8 @@ Since the server runs usings sockets, it is possible to run the server on a different machine than the one running the optimization. This is useful when the optimization is run on a cluster and the plotting server is run on a local machine. -On Macos, this server is necessary as it won't connect using multiprocess. One can simply run the current script on -another terminal to access the online graphs +On Macos, this script is still useful if one wants to force OnlineOptim.SERVER explicitly or run the plotting server +on a different machine. """ from bioptim import PlottingServer diff --git a/tests/shard1/test_biorbd_model.py b/tests/shard1/test_biorbd_model.py deleted file mode 100644 index 2b7815483..000000000 --- a/tests/shard1/test_biorbd_model.py +++ /dev/null @@ -1,123 +0,0 @@ -""" -Test for file IO. -It tests that a model path with another type than string or biorbdmodel return an error -""" - -import biorbd_casadi as biorbd -from bioptim import BiorbdModel -import pytest -import numpy as np -import numpy.testing as npt - -from ..utils import TestUtils - - -def test_biorbd_model_import(): - - bioptim_folder = TestUtils.bioptim_folder() - model_path = "/examples/models/pendulum.bioMod" - BiorbdModel(bioptim_folder + model_path) - - BiorbdModel(biorbd.Model(bioptim_folder + model_path)) - - with pytest.raises(ValueError, match="The model should be of type 'str' or 'biorbd.Model'"): - BiorbdModel(1) - - with pytest.raises(ValueError, match="The model should be of type 'str' or 'biorbd.Model'"): - BiorbdModel([]) - - -@pytest.mark.parametrize( - "my_keys", - [ - ["q"], - ["qdot"], - ["qddot"], - ["q", "qdot"], - ["qdot", "q"], - ["q", "qdot", "qddot"], - ["qddot", "q", "qdot"], - ["qdot", "qddot", "q"], - ["q", "qddot", "qdot"], - ["qddot", "qdot", "q"], - ["qdot", "q", "qddot"], - ], -) -def test_bounds_from_ranges(my_keys): - from bioptim.examples.getting_started import basic_ocp as ocp_module - - x_min_q = [[-1.0, -1.0, -1.0], [-6.28318531, -6.28318531, -6.28318531]] - x_min_qdot = [[-31.41592654, -31.41592654, -31.41592654], [-31.41592654, -31.41592654, -31.41592654]] - x_min_qddot = [[-314.15926536, -314.15926536, -314.15926536], [-314.15926536, -314.15926536, -314.15926536]] - x_max_q = [[5.0, 5.0, 5.0], [6.28318531, 6.28318531, 6.28318531]] - x_max_qdot = [[31.41592654, 31.41592654, 31.41592654], [31.41592654, 31.41592654, 31.41592654]] - x_max_qddot = [[314.15926536, 314.15926536, 314.15926536], [314.15926536, 314.15926536, 314.15926536]] - - bioptim_folder = TestUtils.bioptim_folder() - model_path = "/examples/models/pendulum.bioMod" - bio_model = BiorbdModel(bioptim_folder + model_path) - - for key in my_keys: - x_bounds = bio_model.bounds_from_ranges(key) - - if key == "q": - x_min = np.array(x_min_q) - x_max = np.array(x_max_q) - - elif key == "qdot": - x_min = np.array(x_min_qdot) - x_max = np.array(x_max_qdot) - - elif key == "qddot": - x_min = np.array(x_min_qddot) - x_max = np.array(x_max_qddot) - - else: - raise ValueError("Wrong key") - - # Check min and max have the right value - npt.assert_almost_equal(x_bounds.min, x_min) - npt.assert_almost_equal(x_bounds.max, x_max) - - -def test_function_cached(): - - bioptim_folder = TestUtils.bioptim_folder() - model_path = "/examples/models/pendulum.bioMod" - bio_model = BiorbdModel(bioptim_folder + model_path) - - # No cached function - assert len(bio_model._cached_functions.keys()) == 0 - - # CoM (no argument in the first parenthesis) - # First call create the function and cache it - com = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) - com_id = id(bio_model._cached_functions[("center_of_mass", (), frozenset())]) - assert len(bio_model._cached_functions.keys()) == 1 - - # Second call use the cached function - com = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) - assert com_id == id(bio_model._cached_functions[("center_of_mass", (), frozenset())]) - assert len(bio_model._cached_functions.keys()) == 1 - - # marker (with an argument in the first parenthesis) - # First call create the function and cache it - marker = bio_model.marker(index=0)(bio_model.q, bio_model.parameters) - marker_id = id(bio_model._cached_functions[("marker", (), frozenset({("index", 0)}))]) - assert len(bio_model._cached_functions.keys()) == 2 - - # Second call use the cached function - marker = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) - assert marker_id == id(bio_model._cached_functions[("marker", (), frozenset({("index", 0)}))]) - assert len(bio_model._cached_functions.keys()) == 2 - - # First call with other argument creates the function again and cache it - marker2 = bio_model.marker(index=1)(bio_model.q, bio_model.parameters) - marker_id2 = id(bio_model._cached_functions[("marker", (), frozenset({("index", 1)}))]) - assert len(bio_model._cached_functions.keys()) == 3 - assert marker_id != marker_id2 - - # Second call use the cached function - marker2 = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) - assert marker_id2 == id(bio_model._cached_functions[("marker", (), frozenset({("index", 1)}))]) - assert len(bio_model._cached_functions.keys()) == 3 diff --git a/tests/shard2/test_global_nmpc_final.py b/tests/shard1/test_global_nmpc_final.py similarity index 100% rename from tests/shard2/test_global_nmpc_final.py rename to tests/shard1/test_global_nmpc_final.py diff --git a/tests/shard3/test_graph.py b/tests/shard1/test_graph.py similarity index 100% rename from tests/shard3/test_graph.py rename to tests/shard1/test_graph.py diff --git a/tests/shard3/test_plot.py b/tests/shard1/test_plot.py similarity index 100% rename from tests/shard3/test_plot.py rename to tests/shard1/test_plot.py diff --git a/tests/shard5/test_plot_server.py b/tests/shard1/test_plot_server.py similarity index 100% rename from tests/shard5/test_plot_server.py rename to tests/shard1/test_plot_server.py diff --git a/tests/shard4/test_plot_show_bounds.py b/tests/shard1/test_plot_show_bounds.py similarity index 100% rename from tests/shard4/test_plot_show_bounds.py rename to tests/shard1/test_plot_show_bounds.py diff --git a/tests/shard6/test_plots.py b/tests/shard1/test_plots.py similarity index 100% rename from tests/shard6/test_plots.py rename to tests/shard1/test_plots.py diff --git a/tests/shard4/test_simulate.py b/tests/shard1/test_simulate.py similarity index 100% rename from tests/shard4/test_simulate.py rename to tests/shard1/test_simulate.py diff --git a/tests/shard3/test_global_getting_started.py b/tests/shard2/test_global_getting_started.py similarity index 96% rename from tests/shard3/test_global_getting_started.py rename to tests/shard2/test_global_getting_started.py index c72e8475f..073325241 100644 --- a/tests/shard3/test_global_getting_started.py +++ b/tests/shard2/test_global_getting_started.py @@ -56,10 +56,7 @@ "defects_type", [DefectType.QDDOT_EQUALS_FORWARD_DYNAMICS, DefectType.TAU_EQUALS_INVERSE_DYNAMICS], ) -@pytest.mark.parametrize( - "solver", - [Solver.IPOPT, Solver.FATROP], -) +@pytest.mark.parametrize("solver", [Solver.IPOPT, Solver.FATROP]) @pytest.mark.parametrize("ordering_strategy", [OrderingStrategy.TIME_MAJOR, OrderingStrategy.VARIABLE_MAJOR]) def test_pendulum(ode_solver, use_sx, n_threads, phase_dynamics, defects_type, solver, ordering_strategy): from bioptim.examples.getting_started import basic_ocp as ocp_module @@ -745,13 +742,28 @@ def test_phase_transitions(ode_solver, phase_dynamics): @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("ode_solver", [OdeSolver.RK4, OdeSolver.RK8, OdeSolver.COLLOCATION]) # OdeSolver.IRK def test_parameter_optimization(ode_solver, phase_dynamics): - return # TODO: Fix parameter scaling :( + from bioptim.examples.getting_started import custom_parameters as ocp_module + + if ( + platform.system() == "Darwin" + and ode_solver == OdeSolver.COLLOCATION + and phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE + ): + pytest.skip("Test fails on CI for this particular configuration") + # For reducing time phase_dynamics == PhaseDynamics.ONE_PER_NODE is skipped for redundant tests if phase_dynamics == PhaseDynamics.ONE_PER_NODE and ode_solver in (OdeSolver.RK8, OdeSolver.COLLOCATION): pytest.skip("PhaseDynamics.ONE_PER_NODE is only tested with RK4 and IRK to reduce time") bioptim_folder = TestUtils.bioptim_folder() + gc.collect() # Force garbage collection + time.sleep(0.1) # Avoiding delay in memory (re)allocation + tracemalloc.start() # Start memory tracking + mem_before = tracemalloc.take_snapshot() + + tik = time.time() # Time before starting to build the problem + ode_solver_orig = ode_solver ode_solver = ode_solver() ocp = ocp_module.prepare_ocp( @@ -764,7 +776,7 @@ def test_parameter_optimization(ode_solver, phase_dynamics): max_g=np.array([1, 1, -5]), min_m=10, max_m=30, - target_g=np.array([0, 0, -9.81]), + target_g=np.array([0, 0, -9.81])[:, np.newaxis], target_m=20, ode_solver=ode_solver, phase_dynamics=phase_dynamics, @@ -795,14 +807,14 @@ def test_parameter_optimization(ode_solver, phase_dynamics): npt.assert_equal(g.shape, (80, 1)) npt.assert_almost_equal(g, np.zeros((80, 1)), decimal=6) - TestUtils.assert_objective_value(sol=sol, expected_value=55.29552160879171, decimal=6) + TestUtils.assert_objective_value(sol=sol, expected_value=55.29552160879141, decimal=6) # initial and final controls npt.assert_almost_equal(tau[:, 0], np.array((7.08951794, 0.0))) npt.assert_almost_equal(tau[:, -1], np.array((-15.21533398, 0.0))) # gravity parameter - npt.assert_almost_equal(gravity, np.array([[0, 4.95762449e-03, -9.93171691e00]]).T) + npt.assert_almost_equal(gravity, np.array([0, 4.95762449e-03, -9.93171691e00])) elif isinstance(ode_solver, OdeSolver.RK8): npt.assert_equal(g.shape, (80, 1)) @@ -815,20 +827,20 @@ def test_parameter_optimization(ode_solver, phase_dynamics): npt.assert_almost_equal(tau[:, -1], np.array((-13.06649769, 0.0))) # gravity parameter - npt.assert_almost_equal(gravity, np.array([[0, 5.19787253e-03, -9.84722491e00]]).T) + npt.assert_almost_equal(gravity, np.array([0, 5.19787253e-03, -9.84722491e00])) else: npt.assert_equal(g.shape, (400, 1)) npt.assert_almost_equal(g, np.zeros((400, 1)), decimal=6) - TestUtils.assert_objective_value(sol=sol, expected_value=100.59286910162214, decimal=6) + TestUtils.assert_objective_value(sol=sol, expected_value=100.40019797776976, decimal=6) # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array((-0.23081842, 0.0))) - npt.assert_almost_equal(tau[:, -1], np.array((-26.01316438, 0.0))) + npt.assert_almost_equal(tau[:, 0], np.array((2.02781611, 0.0))) + npt.assert_almost_equal(tau[:, -1], np.array((-24.2402078, 0.0))) # gravity parameter - npt.assert_almost_equal(gravity, np.array([[0, 6.82939855e-03, -1.00000000e01]]).T) + npt.assert_almost_equal(gravity, np.array([0, 7.3139564e-03, -1.0000000e01])) # simulate TestUtils.simulate(sol, decimal_value=6) @@ -1164,6 +1176,77 @@ def test_example_multiphase(ode_solver_type, phase_dynamics): test_memory[f"multiphase-{ode_solver}-{phase_dynamics}"] = [building_duration, solving_duration, mem_used] +@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) +@pytest.mark.parametrize("n_threads", [1, 2]) +@pytest.mark.parametrize("ordering_strategy", [OrderingStrategy.TIME_MAJOR, OrderingStrategy.VARIABLE_MAJOR]) +def test_example_pinocchio(n_threads, phase_dynamics, ordering_strategy): + from bioptim.examples.getting_started import example_pinocchio as ocp_module + + if n_threads > 1 and phase_dynamics == PhaseDynamics.ONE_PER_NODE: + pytest.skip("PhaseDynamics.ONE_PER_NODE is not compatible with multi-threading") + + gc.collect() # Force garbage collection + time.sleep(0.1) # Avoiding delay in memory (re)allocation + tracemalloc.start() # Start memory tracking + mem_before = tracemalloc.take_snapshot() + + tik = time.time() # Time before starting to build the problem + + bioptim_folder = TestUtils.bioptim_folder() + + ocp = ocp_module.prepare_ocp( + model_path=bioptim_folder + "/examples/models/pendulum.urdf", + final_time=1, + n_shooting=30, + n_threads=n_threads, + phase_dynamics=phase_dynamics, + ordering_strategy=ordering_strategy, + ) + tak = time.time() # Time after building, but before solving + ocp.print(to_console=True, to_graph=False) + + sol = ocp.solve(Solver.IPOPT()) + tok = time.time() # This after solving + + # Check constraints + g = np.array(sol.constraints) + npt.assert_equal(g.shape, (120, 1)) + npt.assert_almost_equal(g, np.zeros((120, 1))) + + # Check some of the results + states = sol.decision_states(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + q, qdot, tau = states["q"], states["qdot"], controls["tau"] + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array((0, 0))) + npt.assert_almost_equal(q[:, -1], np.array((0, 3.14))) + + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array((0, 0))) + npt.assert_almost_equal(qdot[:, -1], np.array((0, 0))) + + # simulate + TestUtils.simulate(sol) + + # Execution times + building_duration = tak - tik + solving_duration = tok - tak + + time.sleep(0.1) # Avoiding delay in memory (re)allocation + mem_after = tracemalloc.take_snapshot() + top_stats = mem_after.compare_to(mem_before, "lineno") + mem_used = sum(stat.size_diff for stat in top_stats) + tracemalloc.stop() + + global test_memory + test_memory[f"pendulum-{n_threads}-{phase_dynamics}"] = [ + building_duration, + solving_duration, + mem_used, + ] + + @pytest.mark.parametrize("expand_dynamics", [True, False]) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("ode_solver", [OdeSolver.RK4, OdeSolver.IRK]) diff --git a/tests/shard2/test_custom_weight.py b/tests/shard3/test_custom_weight.py similarity index 100% rename from tests/shard2/test_custom_weight.py rename to tests/shard3/test_custom_weight.py diff --git a/tests/shard4/test_biorbd_model.py b/tests/shard4/test_biorbd_model.py new file mode 100644 index 000000000..e5bab1c4c --- /dev/null +++ b/tests/shard4/test_biorbd_model.py @@ -0,0 +1,347 @@ +""" +Test for file IO. +It tests that a model path with another type than string or biorbdmodel return an error +""" + +import biorbd_casadi as biorbd +from bioptim import BiorbdModel +import pytest +import numpy as np +import numpy.testing as npt + +from ..utils import TestUtils + + +def _build_biorbd_model(relative_model_path: str) -> BiorbdModel: + return BiorbdModel(TestUtils.bioptim_folder() + relative_model_path) + + +def test_biorbd_model_import(): + + bioptim_folder = TestUtils.bioptim_folder() + model_path = "/examples/models/pendulum.bioMod" + BiorbdModel(bioptim_folder + model_path) + + BiorbdModel(biorbd.Model(bioptim_folder + model_path)) + + with pytest.raises(ValueError, match="The model should be of type 'str' or 'biorbd.Model'"): + BiorbdModel(1) + + with pytest.raises(ValueError, match="The model should be of type 'str' or 'biorbd.Model'"): + BiorbdModel([]) + + +@pytest.mark.parametrize( + "my_keys", + [ + ["q"], + ["qdot"], + ["qddot"], + ["q", "qdot"], + ["qdot", "q"], + ["q", "qdot", "qddot"], + ["qddot", "q", "qdot"], + ["qdot", "qddot", "q"], + ["q", "qddot", "qdot"], + ["qddot", "qdot", "q"], + ["qdot", "q", "qddot"], + ], +) +def test_bounds_from_ranges(my_keys): + x_min_q = [[-1.0, -1.0, -1.0], [-6.28318531, -6.28318531, -6.28318531]] + x_min_qdot = [[-31.41592654, -31.41592654, -31.41592654], [-31.41592654, -31.41592654, -31.41592654]] + x_min_qddot = [[-314.15926536, -314.15926536, -314.15926536], [-314.15926536, -314.15926536, -314.15926536]] + x_max_q = [[5.0, 5.0, 5.0], [6.28318531, 6.28318531, 6.28318531]] + x_max_qdot = [[31.41592654, 31.41592654, 31.41592654], [31.41592654, 31.41592654, 31.41592654]] + x_max_qddot = [[314.15926536, 314.15926536, 314.15926536], [314.15926536, 314.15926536, 314.15926536]] + + bioptim_folder = TestUtils.bioptim_folder() + model_path = "/examples/models/pendulum.bioMod" + bio_model = BiorbdModel(bioptim_folder + model_path) + + for key in my_keys: + x_bounds = bio_model.bounds_from_ranges(key) + + if key == "q": + x_min = np.array(x_min_q) + x_max = np.array(x_max_q) + + elif key == "qdot": + x_min = np.array(x_min_qdot) + x_max = np.array(x_max_qdot) + + elif key == "qddot": + x_min = np.array(x_min_qddot) + x_max = np.array(x_max_qddot) + + else: + raise ValueError("Wrong key") + + # Check min and max have the right value + npt.assert_almost_equal(x_bounds.min, x_min) + npt.assert_almost_equal(x_bounds.max, x_max) + + +def test_function_cached(): + + bioptim_folder = TestUtils.bioptim_folder() + model_path = "/examples/models/pendulum.bioMod" + bio_model = BiorbdModel(bioptim_folder + model_path) + + # No cached function + assert len(bio_model._cached_functions.keys()) == 0 + + # CoM (no argument in the first parenthesis) + # First call create the function and cache it + com = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) + com_id = id(bio_model._cached_functions[("center_of_mass", (), frozenset())]) + assert len(bio_model._cached_functions.keys()) == 1 + + # Second call use the cached function + com = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) + assert com_id == id(bio_model._cached_functions[("center_of_mass", (), frozenset())]) + assert len(bio_model._cached_functions.keys()) == 1 + + # marker (with an argument in the first parenthesis) + # First call create the function and cache it + marker = bio_model.marker(index=0)(bio_model.q, bio_model.parameters) + marker_id = id(bio_model._cached_functions[("marker", (), frozenset({("index", 0)}))]) + assert len(bio_model._cached_functions.keys()) == 2 + + # Second call use the cached function + marker = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) + assert marker_id == id(bio_model._cached_functions[("marker", (), frozenset({("index", 0)}))]) + assert len(bio_model._cached_functions.keys()) == 2 + + # First call with other argument creates the function again and cache it + marker2 = bio_model.marker(index=1)(bio_model.q, bio_model.parameters) + marker_id2 = id(bio_model._cached_functions[("marker", (), frozenset({("index", 1)}))]) + assert len(bio_model._cached_functions.keys()) == 3 + assert marker_id != marker_id2 + + # Second call use the cached function + marker2 = bio_model.center_of_mass()(bio_model.q, bio_model.parameters) + assert marker_id2 == id(bio_model._cached_functions[("marker", (), frozenset({("index", 1)}))]) + assert len(bio_model._cached_functions.keys()) == 3 + + +def test_biorbd_model_metadata_copy_and_serialize(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.bioMod" + bio_model = BiorbdModel(model_path) + + assert bio_model.path.replace("\\", "/") == model_path.replace("\\", "/") + assert bio_model.name == "pendulum.bioMod" + + model_copy = bio_model.copy() + assert isinstance(model_copy, BiorbdModel) + assert model_copy is not bio_model + assert model_copy.path == bio_model.path + + serializer, data = bio_model.serialize() + assert serializer is BiorbdModel + assert data["bio_model"].replace("\\", "/") == model_path.replace("\\", "/") + assert data["external_force_set"] is None + + +def test_biorbd_gravity_setter_updates_value(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.bioMod" + bio_model = BiorbdModel(model_path) + + new_gravity = np.array([0.1, -0.2, -3.5]) + bio_model.set_gravity(new_gravity) + + updated_gravity = np.array(bio_model.gravity()(np.zeros((0, 1)))).squeeze() + npt.assert_almost_equal(updated_gravity, new_gravity) + + +def test_biorbd_set_friction_coefficients_validation_and_shape(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.bioMod" + bio_model = BiorbdModel(model_path) + + assert bio_model.friction_coefficients is None + + valid_coefficients = np.ones((bio_model.nb_tau, bio_model.nb_qdot)) + bio_model.set_friction_coefficients(valid_coefficients) + npt.assert_almost_equal(bio_model.friction_coefficients, valid_coefficients) + + with pytest.raises(ValueError, match="Friction coefficients must be positive"): + bio_model.set_friction_coefficients(-valid_coefficients) + + +def test_biorbd_tau_max_output_shape_and_signs(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/cube_with_actuators.bioMod" + bio_model = BiorbdModel(model_path) + + q = np.zeros((bio_model.nb_q, 1)) + qdot = np.zeros((bio_model.nb_qdot, 1)) + parameters = np.zeros((0, 1)) + + tau_max, tau_min = bio_model.tau_max()(q, qdot, parameters) + tau_max = np.array(tau_max) + tau_min = np.array(tau_min) + + assert tau_max.shape == (bio_model.nb_tau, 1) + assert tau_min.shape == (bio_model.nb_tau, 1) + assert np.all(tau_max >= tau_min) + + +def test_biorbd_basic_properties_and_name_consistency(): + bio_model = _build_biorbd_model("/examples/models/pendulum.bioMod") + + assert bio_model.nb_q == bio_model.nb_qdot == bio_model.nb_qddot == bio_model.nb_tau == bio_model.nb_dof + assert bio_model.nb_segments == len(bio_model.segments) + assert len(bio_model.name_dofs) == bio_model.nb_dof + assert len(bio_model.marker_names) == bio_model.nb_markers + assert bio_model.nb_muscles == len(bio_model.muscle_names) + assert bio_model.nb_soft_contacts == len(bio_model.soft_contact_names) + + if bio_model.nb_markers > 0: + first_marker_name = bio_model.marker_names[0] + marker_idx = bio_model.marker_index(first_marker_name) + assert 0 <= marker_idx < bio_model.nb_markers + + +def test_biorbd_kinematics_and_dynamics_functions_shapes(): + bio_model = _build_biorbd_model("/examples/models/pendulum.bioMod") + + q = np.zeros((bio_model.nb_q, 1)) + qdot = np.zeros((bio_model.nb_qdot, 1)) + qddot = np.zeros((bio_model.nb_qddot, 1)) + tau = np.zeros((bio_model.nb_tau, 1)) + external_forces = np.zeros((0, 1)) + parameters = np.zeros((0, 1)) + + mass_value = np.array(bio_model.mass()(parameters)).squeeze() + assert np.isfinite(mass_value) + assert float(mass_value) > 0 + + com = np.array(bio_model.center_of_mass()(q, parameters)) + com_velocity = np.array(bio_model.center_of_mass_velocity()(q, qdot, parameters)) + com_acceleration = np.array(bio_model.center_of_mass_acceleration()(q, qdot, qddot, parameters)) + assert com.shape == (3, 1) + assert com_velocity.shape == (3, 1) + assert com_acceleration.shape == (3, 1) + + mass_matrix = np.array(bio_model.mass_matrix()(q, parameters)) + non_linear_effects = np.array(bio_model.non_linear_effects()(q, qdot, parameters)) + angular_momentum = np.array(bio_model.angular_momentum()(q, qdot, parameters)) + reshaped_qdot = np.array(bio_model.reshape_qdot()(q, qdot, parameters)) + lagrangian = np.array(bio_model.lagrangian()(q, qdot)).squeeze() + + assert mass_matrix.shape == (bio_model.nb_q, bio_model.nb_q) + assert non_linear_effects.shape == (bio_model.nb_q, 1) + assert angular_momentum.shape == (3, 1) + assert reshaped_qdot.shape == (bio_model.nb_qdot, 1) + assert np.isfinite(float(lagrangian)) + + forward_dynamics = np.array(bio_model.forward_dynamics()(q, qdot, tau, external_forces, parameters)) + inverse_dynamics = np.array(bio_model.inverse_dynamics()(q, qdot, qddot, external_forces, parameters)) + assert forward_dynamics.shape == (bio_model.nb_qddot, 1) + assert inverse_dynamics.shape == (bio_model.nb_tau, 1) + + +def test_biorbd_rotation_and_homogeneous_matrix_utilities(): + bio_model = _build_biorbd_model("/examples/models/pendulum.bioMod") + + q = np.zeros((bio_model.nb_q, 1)) + parameters = np.zeros((0, 1)) + + euler_angles = np.array(bio_model.rotation_matrix_to_euler_angles("xyz")(np.eye(3))).squeeze() + npt.assert_allclose(euler_angles, np.zeros((3,)), atol=1e-12) + + homogeneous = np.array(bio_model.homogeneous_matrices_in_global(0)(q, parameters)) + homogeneous_inverse = np.array(bio_model.homogeneous_matrices_in_global(0, inverse=True)(q, parameters)) + local_homogeneous = np.array(bio_model.homogeneous_matrices_in_child(0)(parameters)) + assert homogeneous.shape == (4, 4) + assert homogeneous_inverse.shape == (4, 4) + assert local_homogeneous.shape == (4, 4) + npt.assert_allclose(homogeneous @ homogeneous_inverse, np.eye(4), atol=1e-8) + + +def test_biorbd_segment_orientation_currently_raises_on_casadi_types(): + bio_model = _build_biorbd_model("/examples/models/pendulum.bioMod") + q = np.zeros((bio_model.nb_q, 1)) + parameters = np.zeros((0, 1)) + + with pytest.raises(NotImplementedError): + bio_model.segment_orientation(0)(q, parameters) + + +def test_biorbd_marker_related_outputs_are_consistent(): + bio_model = _build_biorbd_model("/examples/models/pendulum.bioMod") + if bio_model.nb_markers == 0: + pytest.skip("This model has no markers") + + q = np.zeros((bio_model.nb_q, 1)) + qdot = np.zeros((bio_model.nb_qdot, 1)) + qddot = np.zeros((bio_model.nb_qddot, 1)) + parameters = np.zeros((0, 1)) + + markers = np.array(bio_model.markers()(q, parameters)) + marker0 = np.array(bio_model.marker(0)(q, parameters)) + marker0_velocity = np.array(bio_model.marker_velocity(0)(q, qdot, parameters)) + marker0_acceleration = np.array(bio_model.marker_acceleration(0)(q, qdot, qddot, parameters)) + markers_velocities = np.array(bio_model.markers_velocities()(q, qdot, parameters)) + markers_accelerations = np.array(bio_model.markers_accelerations()(q, qdot, qddot, parameters)) + + assert markers.shape == (3, bio_model.nb_markers) + assert marker0.shape == (3, 1) + assert marker0_velocity.shape == (3, 1) + assert marker0_acceleration.shape == (3, 1) + assert markers_velocities.shape == (3, bio_model.nb_markers) + assert markers_accelerations.shape == (3, bio_model.nb_markers) + npt.assert_allclose(marker0.squeeze(), markers[:, 0], atol=1e-12) + + with pytest.raises(RuntimeError, match="Mismatching number of output names"): + bio_model.markers_jacobian()(q, parameters) + + +def test_biorbd_rigid_contact_methods_shapes(): + bio_model = _build_biorbd_model("/examples/models/2segments_2dof_2contacts.bioMod") + assert bio_model.nb_rigid_contacts > 0 + assert bio_model.nb_contacts > 0 + + q = np.zeros((bio_model.nb_q, 1)) + qdot = np.zeros((bio_model.nb_qdot, 1)) + qddot = np.zeros((bio_model.nb_qddot, 1)) + tau = np.zeros((bio_model.nb_tau, 1)) + external_forces = np.zeros((0, 1)) + parameters = np.zeros((0, 1)) + + rigid_contact_names = bio_model.rigid_contact_names + if len(rigid_contact_names) > 0: + assert 0 <= bio_model.contact_index(rigid_contact_names[0]) < bio_model.nb_contacts + + for contact_idx in range(bio_model.nb_rigid_contacts): + axes = bio_model.rigid_contact_axes_index(contact_idx) + assert len(axes) > 0 + assert isinstance(bio_model.rigid_contact_segment(contact_idx), str) + + velocity = np.array(bio_model.rigid_contact_velocity(contact_idx)(q, qdot, parameters)) + velocity_on_available_axes = np.array( + bio_model.rigid_contact_velocity(contact_idx, contact_axis=tuple(axes))(q, qdot, parameters) + ) + acceleration_on_first_axis = np.array( + bio_model.rigid_contact_acceleration(contact_idx, (axes[0],))(q, qdot, qddot, parameters) + ) + + assert velocity.shape == (3, 1) + assert velocity_on_available_axes.shape == (len(axes), 1) + assert acceleration_on_first_axis.shape == (1, 1) + + +def test_biorbd_inverse_dynamics_with_contact_not_implemented(): + bio_model = _build_biorbd_model("/examples/models/pendulum.bioMod") + with pytest.raises(NotImplementedError, match="Inverse dynamics with contact is not implemented yet"): + bio_model.inverse_dynamics(with_contact=True) + + +def test_biorbd_reorder_qddot_root_joints_static_method(): + qddot_root = np.array([[1.0], [2.0]]) + qddot_joints = np.array([[3.0]]) + reordered = BiorbdModel.reorder_qddot_root_joints(qddot_root, qddot_joints) + npt.assert_allclose(np.array(reordered), np.array([[1.0], [2.0], [3.0]])) diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard4/test_biorbd_model_holonomic.py similarity index 61% rename from tests/shard1/test_biorbd_model_holonomic.py rename to tests/shard4/test_biorbd_model_holonomic.py index 253429502..f14339d4b 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard4/test_biorbd_model_holonomic.py @@ -1277,3 +1277,744 @@ def test_example_arm26_pendulum_swingup_muscle_algebraic(): ), decimal=6, ) + + +def test_example_alignment_constraint(): + """Test the holonomic_constraints/frame_alignment example""" + from bioptim.examples.toy_examples.holonomic_constraints import frame_alignment_orientation + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, bio_model = frame_alignment_orientation.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/two_cubes_lagrange2D_outofplane.bioMod", + n_shooting=10, + final_time=2.0, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + q, _, _, _ = frame_alignment_orientation.compute_all_states(sol, bio_model) + + print(q) + + npt.assert_almost_equal( + q, + np.array( + [ + [0.0, 0.01, 0.04, 0.09, 0.16, 0.25, 0.36, 0.49, 0.64, 0.81, 1.0], + [ + 0.44879895, + 0.45819223, + 0.48660965, + 0.53479017, + 0.60404417, + 0.69633279, + 0.81427079, + 0.9608684, + 1.1386939, + 1.34814259, + 1.58512334, + ], + [ + -0.39269908, + -0.39768345, + -0.41244445, + -0.43637074, + -0.46832351, + -0.50644603, + -0.54788349, + -0.58842906, + -0.62219113, + -0.64154838, + -0.6378551, + ], + [ + -0.52359878, + -0.51998247, + -0.50878286, + -0.48893843, + -0.45865539, + -0.41540327, + -0.35600627, + -0.27700848, + -0.17562772, + -0.05161501, + 0.09027961, + ], + ] + ), + decimal=6, + ) + + +def test_example_generalized_alignment_constraint(): + """Test the holonomic_constraints/frame_alignment example""" + from bioptim.examples.toy_examples.holonomic_constraints import frame_alignment_orientation_6DOF + + bioptim_folder = TestUtils.bioptim_folder() + + n_shooting = 10 + + interpolated_points = frame_alignment_orientation_6DOF.build_dummy_trajectory_for_the_driving_cube(n_shooting) + + # --- Prepare the ocp --- # + ocp = frame_alignment_orientation_6DOF.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/two_cubes_lagrange2D_6DOF.bioMod", + n_shooting=n_shooting, + final_time=1.0, + interpolated_points=interpolated_points, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + 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() + + npt.assert_almost_equal( + q, + np.array( + [ + [ + 0.00000000e00, + 1.84183688e-02, + 8.69533563e-02, + 1.74878304e-01, + 2.41034127e-01, + 2.58471333e-01, + 2.75800263e-01, + 3.39618878e-01, + 4.19924054e-01, + 4.79143834e-01, + 4.94576090e-01, + 5.09804824e-01, + 5.64631094e-01, + 6.30606639e-01, + 6.76895124e-01, + 6.88601731e-01, + 6.99962889e-01, + 7.38794655e-01, + 7.80446328e-01, + 8.05503063e-01, + 8.11180516e-01, + 8.16375325e-01, + 8.31013400e-01, + 8.38669494e-01, + 8.35872495e-01, + 8.33872112e-01, + 8.31360572e-01, + 8.17528387e-01, + 7.89167394e-01, + 7.59682447e-01, + 7.50710849e-01, + 7.41351426e-01, + 7.03492484e-01, + 6.47783856e-01, + 6.00417441e-01, + 5.87131933e-01, + 5.73646647e-01, + 5.21962799e-01, + 4.52113357e-01, + 3.96840261e-01, + 3.81869496e-01, + 3.66848273e-01, + 3.10481533e-01, + 2.37007346e-01, + 1.80774370e-01, + 1.65803663e-01, + 1.50855044e-01, + 9.50955518e-02, + 2.31169400e-02, + -3.15199368e-02, + -4.60095466e-02, + ], + [ + 0.00000000e00, + 1.52195502e-02, + 7.04221945e-02, + 1.37873836e-01, + 1.86034800e-01, + 1.98344622e-01, + 2.10434143e-01, + 2.53837566e-01, + 3.05728932e-01, + 3.41798442e-01, + 3.50858164e-01, + 3.59685678e-01, + 3.90738731e-01, + 4.26220292e-01, + 4.49451401e-01, + 4.55048672e-01, + 4.60395149e-01, + 4.78207858e-01, + 4.95971679e-01, + 5.05265803e-01, + 5.07095915e-01, + 5.08649722e-01, + 5.11975699e-01, + 5.10242071e-01, + 5.04145384e-01, + 5.01810932e-01, + 4.99171598e-01, + 4.86492511e-01, + 4.63277268e-01, + 4.40300335e-01, + 4.33412990e-01, + 4.26201211e-01, + 3.96224192e-01, + 3.50200390e-01, + 3.09647271e-01, + 2.98073348e-01, + 2.86177374e-01, + 2.38675918e-01, + 1.70011949e-01, + 1.12365469e-01, + 9.62843392e-02, + 7.99033422e-02, + 1.57853353e-02, + -7.39840143e-02, + -1.47313278e-01, + -1.67495822e-01, + -1.87944551e-01, + -2.67020368e-01, + -3.75553741e-01, + -4.62678911e-01, + -4.86451673e-01, + ], + [ + 0.00000000e00, + 2.28995880e-02, + 1.05223199e-01, + 2.04086355e-01, + 2.73356394e-01, + 2.90866995e-01, + 3.07979846e-01, + 3.68662489e-01, + 4.39479516e-01, + 4.87403344e-01, + 4.99248313e-01, + 5.10705293e-01, + 5.50252878e-01, + 5.93711667e-01, + 6.20831039e-01, + 6.27156119e-01, + 6.33103712e-01, + 6.52074381e-01, + 6.68920270e-01, + 6.75817206e-01, + 6.76778831e-01, + 6.77374223e-01, + 6.76364689e-01, + 6.67381642e-01, + 6.54650300e-01, + 6.50404528e-01, + 6.45801264e-01, + 6.25350257e-01, + 5.91134344e-01, + 5.59132073e-01, + 5.49757523e-01, + 5.40024689e-01, + 5.00282566e-01, + 4.40746180e-01, + 3.89165837e-01, + 3.74543230e-01, + 3.59546502e-01, + 2.99885050e-01, + 2.13923433e-01, + 1.41715600e-01, + 1.21538179e-01, + 1.00961888e-01, + 2.01412539e-02, + -9.39556377e-02, + -1.88133332e-01, + -2.14223310e-01, + -2.40735390e-01, + -3.44025644e-01, + -4.87869855e-01, + -6.05145894e-01, + -6.37430021e-01, + ], + [ + 0.00000000e00, + 1.11247018e-02, + 4.95949696e-02, + 9.15952168e-02, + 1.17133856e-01, + 1.22924936e-01, + 1.28281102e-01, + 1.44433299e-01, + 1.55696423e-01, + 1.56308418e-01, + 1.55231633e-01, + 1.53622566e-01, + 1.42764836e-01, + 1.16396507e-01, + 8.60992059e-02, + 7.64672370e-02, + 6.61757230e-02, + 2.17188925e-02, + -5.04205118e-02, + -1.16228013e-01, + -1.35197885e-01, + -1.54725263e-01, + -2.32668171e-01, + -3.42537649e-01, + -4.29543109e-01, + -4.52668157e-01, + -4.75687410e-01, + -5.60372089e-01, + -6.63128265e-01, + -7.33163230e-01, + -7.50323623e-01, + -7.66820206e-01, + -8.22515605e-01, + -8.80275768e-01, + -9.13374057e-01, + -9.20631714e-01, + -9.27238321e-01, + -9.46365050e-01, + -9.58707999e-01, + -9.59290431e-01, + -9.58230580e-01, + -9.56674384e-01, + -9.46551252e-01, + -9.23774036e-01, + -8.99538966e-01, + -8.92148710e-01, + -8.84380924e-01, + -8.51996152e-01, + -8.02637583e-01, + -7.59987364e-01, + -7.47996280e-01, + ], + [ + 0.00000000e00, + 1.16335926e-02, + 5.80615655e-02, + 1.24340089e-01, + 1.78645194e-01, + 1.93528583e-01, + 2.08558685e-01, + 2.66024627e-01, + 3.42608257e-01, + 4.01608440e-01, + 4.17263645e-01, + 4.32870984e-01, + 4.90835824e-01, + 5.63921189e-01, + 6.16812675e-01, + 6.30303331e-01, + 6.43521306e-01, + 6.90557261e-01, + 7.44376588e-01, + 7.78406571e-01, + 7.86252436e-01, + 7.93566493e-01, + 8.16141062e-01, + 8.32894962e-01, + 8.35297932e-01, + 8.34375595e-01, + 8.32802073e-01, + 8.21117387e-01, + 7.92846696e-01, + 7.62206000e-01, + 7.52879998e-01, + 7.43090178e-01, + 7.02490129e-01, + 6.41944597e-01, + 5.91237234e-01, + 5.77260194e-01, + 5.63113410e-01, + 5.08755403e-01, + 4.36096059e-01, + 3.80235363e-01, + 3.65448277e-01, + 3.50724713e-01, + 2.96222827e-01, + 2.27870112e-01, + 1.78503603e-01, + 1.65891310e-01, + 1.53528837e-01, + 1.09541911e-01, + 5.86720026e-02, + 2.54147046e-02, + 1.74698811e-02, + ], + [ + 0.00000000e00, + 3.02476726e-02, + 1.42759272e-01, + 2.87649334e-01, + 3.97858767e-01, + 4.27187863e-01, + 4.56525716e-01, + 5.66944669e-01, + 7.12785660e-01, + 8.27038273e-01, + 8.57981367e-01, + 8.89162409e-01, + 1.00853310e00, + 1.17123921e00, + 1.30261670e00, + 1.33874869e00, + 1.37537211e00, + 1.51727562e00, + 1.71383379e00, + 1.87344158e00, + 1.91723486e00, + 1.96152778e00, + 2.13201892e00, + 2.36218913e00, + 2.54153889e00, + 2.58934484e00, + 2.63708899e00, + 2.81506775e00, + 3.04123578e00, + 3.20765421e00, + 3.25081714e00, + 3.29346046e00, + 3.44865369e00, + 3.64002609e00, + 3.77898419e00, + 3.81501703e00, + 3.85064739e00, + 3.98099869e00, + 4.14448579e00, + 4.26607470e00, + 4.29807307e00, + 4.32992813e00, + 4.44842218e00, + 4.60154819e00, + 4.71866864e00, + 4.74992639e00, + 4.78122211e00, + 4.89913969e00, + 5.05452899e00, + 5.17508073e00, + 5.20742850e00, + ], + [ + 0.00000000e00, + 1.30038173e-02, + 6.18352909e-02, + 1.25509622e-01, + 1.74180072e-01, + 1.87117015e-01, + 2.00017772e-01, + 2.47906771e-01, + 3.09029378e-01, + 3.54733597e-01, + 3.66733165e-01, + 3.78611106e-01, + 4.21688213e-01, + 4.74234839e-01, + 5.11623640e-01, + 5.21155231e-01, + 5.30439034e-01, + 5.62477091e-01, + 5.97561898e-01, + 6.19263853e-01, + 6.24283805e-01, + 6.28930106e-01, + 6.42564809e-01, + 6.51327306e-01, + 6.51145190e-01, + 6.50091087e-01, + 6.48623976e-01, + 6.39526088e-01, + 6.19204187e-01, + 5.97245968e-01, + 5.90473711e-01, + 5.83378394e-01, + 5.54448411e-01, + 5.11399126e-01, + 4.74506953e-01, + 4.64126550e-01, + 4.53580706e-01, + 4.13116935e-01, + 3.58407645e-01, + 3.15187007e-01, + 3.03501902e-01, + 2.91790546e-01, + 2.48001401e-01, + 1.91409431e-01, + 1.48583791e-01, + 1.37266763e-01, + 1.26005979e-01, + 8.43918380e-02, + 3.17124394e-02, + -7.36968042e-03, + -1.75898471e-02, + ], + [ + -2.00000000e00, + -2.00951339e00, + -2.04475267e00, + -2.08960446e00, + -2.12310700e00, + -2.13190588e00, + -2.14063458e00, + -2.17262301e00, + -2.21255334e00, + -2.24180783e00, + -2.24941021e00, + -2.25689803e00, + -2.28368156e00, + -2.31557096e00, + -2.33775253e00, + -2.34334232e00, + -2.34875036e00, + -2.36701203e00, + -2.38613186e00, + -2.39730699e00, + -2.39979042e00, + -2.40203288e00, + -2.40800830e00, + -2.41016600e00, + -2.40765123e00, + -2.40638818e00, + -2.40489063e00, + -2.39731292e00, + -2.38292954e00, + -2.36859941e00, + -2.36431208e00, + -2.35987685e00, + -2.34234841e00, + -2.31746611e00, + -2.29691625e00, + -2.29122921e00, + -2.28549802e00, + -2.26398771e00, + -2.23589476e00, + -2.21429567e00, + -2.20852305e00, + -2.20276962e00, + -2.18158033e00, + -2.15477062e00, + -2.13472904e00, + -2.12944604e00, + -2.12419255e00, + -2.10478626e00, + -2.08002401e00, + -2.06129091e00, + -2.05631410e00, + ], + [ + 0.00000000e00, + 2.89077537e-02, + 1.33287992e-01, + 2.59732999e-01, + 3.49193478e-01, + 3.71940438e-01, + 3.94226717e-01, + 4.73740491e-01, + 5.67688787e-01, + 6.32186728e-01, + 6.48272450e-01, + 6.63891313e-01, + 7.18308488e-01, + 7.79319124e-01, + 8.18406519e-01, + 8.27695422e-01, + 8.36504431e-01, + 8.65230021e-01, + 8.92390424e-01, + 9.05271810e-01, + 9.07538997e-01, + 9.09310471e-01, + 9.11488494e-01, + 9.03685973e-01, + 8.89518312e-01, + 8.84541230e-01, + 8.79057720e-01, + 8.53955149e-01, + 8.10433189e-01, + 7.68792694e-01, + 7.56481620e-01, + 7.43662811e-01, + 6.91058938e-01, + 6.11725131e-01, + 5.42693722e-01, + 5.23093063e-01, + 5.02988148e-01, + 4.23068895e-01, + 3.08174314e-01, + 2.11957585e-01, + 1.85124957e-01, + 1.57791822e-01, + 5.07692654e-02, + -9.94095665e-02, + -2.22588566e-01, + -2.56590902e-01, + -2.91088696e-01, + -4.24964166e-01, + -6.10067603e-01, + -7.59905222e-01, + -8.00992172e-01, + ], + [ + 4.48798951e-01, + 4.65491412e-01, + 5.25852161e-01, + 5.97809403e-01, + 6.46685808e-01, + 6.58702865e-01, + 6.70281360e-01, + 7.09735915e-01, + 7.51449622e-01, + 7.75788792e-01, + 7.81149213e-01, + 7.86045248e-01, + 8.00388927e-01, + 8.09482039e-01, + 8.09028841e-01, + 8.07809756e-01, + 8.06143872e-01, + 7.96089590e-01, + 7.73733773e-01, + 7.49380387e-01, + 7.41811965e-01, + 7.33797946e-01, + 6.99695242e-01, + 6.45626056e-01, + 5.96944715e-01, + 5.82956944e-01, + 5.68539957e-01, + 5.10614622e-01, + 4.27142232e-01, + 3.58645484e-01, + 3.39965578e-01, + 3.21154829e-01, + 2.50008201e-01, + 1.58526180e-01, + 9.19902716e-02, + 7.50406978e-02, + 5.84751389e-02, + 1.33716754e-04, + -6.55962259e-02, + -1.07198490e-01, + -1.16954684e-01, + -1.26148963e-01, + -1.55678559e-01, + -1.82549514e-01, + -1.94484710e-01, + -1.96434461e-01, + -1.97877817e-01, + -1.98882293e-01, + -1.90054127e-01, + -1.75849311e-01, + -1.71011478e-01, + ], + [ + -3.92699082e-01, + -3.88136818e-01, + -3.66126749e-01, + -3.26610549e-01, + -2.88967908e-01, + -2.77977358e-01, + -2.66615807e-01, + -2.20974972e-01, + -1.55385463e-01, + -1.01544750e-01, + -8.67941019e-02, + -7.18972015e-02, + -1.48966783e-02, + 6.11137254e-02, + 1.19683566e-01, + 1.35229448e-01, + 1.50735822e-01, + 2.08474718e-01, + 2.81809639e-01, + 3.35548770e-01, + 3.49396637e-01, + 3.63035380e-01, + 4.12342992e-01, + 4.71105675e-01, + 5.10841986e-01, + 5.20524860e-01, + 5.29815491e-01, + 5.60985225e-01, + 5.91820229e-01, + 6.07008160e-01, + 6.09737819e-01, + 6.11889027e-01, + 6.14588855e-01, + 6.05369612e-01, + 5.88846687e-01, + 5.83138517e-01, + 5.76888375e-01, + 5.48764017e-01, + 5.02199148e-01, + 4.60233090e-01, + 4.48284144e-01, + 4.36042731e-01, + 3.87766944e-01, + 3.20528727e-01, + 2.66957493e-01, + 2.52516897e-01, + 2.38036497e-01, + 1.83589815e-01, + 1.13375225e-01, + 6.12071303e-02, + 4.76607485e-02, + ], + [ + -5.23598776e-01, + -4.86933516e-01, + -3.50845716e-01, + -1.77685387e-01, + -4.88076633e-02, + -1.50459449e-02, + 1.84755751e-02, + 1.42220569e-01, + 2.99247702e-01, + 4.16842344e-01, + 4.47860822e-01, + 4.78766376e-01, + 5.93919244e-01, + 7.43135273e-01, + 8.57681423e-01, + 8.88343859e-01, + 9.19083971e-01, + 1.03527017e00, + 1.18979889e00, + 1.31139056e00, + 1.34435271e00, + 1.37757005e00, + 1.50476572e00, + 1.67735635e00, + 1.81520423e00, + 1.85276781e00, + 1.89069761e00, + 2.03639996e00, + 2.23383590e00, + 2.38976199e00, + 2.43180948e00, + 2.47402099e00, + 2.63331774e00, + 2.84112961e00, + 2.99836183e00, + 3.03976519e00, + 3.08090797e00, + 3.23262761e00, + 3.42348423e00, + 3.56410421e00, + 3.60076974e00, + 3.63710453e00, + 3.77064406e00, + 3.93899874e00, + 4.06446831e00, + 4.09749315e00, + 4.13037472e00, + 4.25275144e00, + 4.41098259e00, + 4.53208865e00, + 4.56443031e00, + ], + ] + ), + decimal=6, + ) diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard4/test_biorbd_multi_model.py similarity index 100% rename from tests/shard1/test_biorbd_multi_model.py rename to tests/shard4/test_biorbd_multi_model.py diff --git a/tests/shard2/test_cost_function_integration.py b/tests/shard4/test_cost_function_integration.py similarity index 100% rename from tests/shard2/test_cost_function_integration.py rename to tests/shard4/test_cost_function_integration.py diff --git a/tests/shard6/test_dt_dependent.py b/tests/shard4/test_dt_dependent.py similarity index 100% rename from tests/shard6/test_dt_dependent.py rename to tests/shard4/test_dt_dependent.py diff --git a/tests/shard3/test_passive_torque.py b/tests/shard4/test_passive_torque.py similarity index 100% rename from tests/shard3/test_passive_torque.py rename to tests/shard4/test_passive_torque.py diff --git a/tests/shard1/test_prepare_all_examples.py b/tests/shard4/test_prepare_all_examples.py similarity index 99% rename from tests/shard1/test_prepare_all_examples.py rename to tests/shard4/test_prepare_all_examples.py index d431ba71c..a703e6657 100644 --- a/tests/shard1/test_prepare_all_examples.py +++ b/tests/shard4/test_prepare_all_examples.py @@ -319,6 +319,20 @@ def test__getting_started__example_multiphase(): ) +def test__getting_started__example_pinocchio(): + from bioptim.examples.getting_started import example_pinocchio as ocp_module + + bioptim_folder = TestUtils.bioptim_folder() + + ocp_module.prepare_ocp( + model_path=bioptim_folder + "/examples/models/pendulum.urdf", + final_time=3, + n_shooting=100, + phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics=False, + ) + + def test__getting_started__example_multiphase_different_ode_solvers(): from bioptim.examples.getting_started import example_multiphase as ocp_module diff --git a/tests/shard4/test_solver_options.py b/tests/shard4/test_solver_options.py deleted file mode 100644 index 71d499036..000000000 --- a/tests/shard4/test_solver_options.py +++ /dev/null @@ -1,133 +0,0 @@ -from bioptim import Solver -from bioptim.misc.enums import SolverType - - -class FakeSolver: - def __init__( - self, - options_common: dict = None, - ): - self.options_common = options_common - - -def test_ipopt_solver_options(): - solver = Solver.IPOPT() - assert solver.type == SolverType.IPOPT - assert solver.show_online_optim is None - assert solver.online_optim is None - assert solver.show_options is None - assert solver.tol == 1e-6 - assert solver.dual_inf_tol == 1.0 - assert solver.constr_viol_tol == 0.0001 - assert solver.compl_inf_tol == 0.0001 - assert solver.acceptable_tol == 1e-6 - assert solver.acceptable_dual_inf_tol == 1e10 - assert solver.acceptable_constr_viol_tol == 1e-2 - assert solver.acceptable_compl_inf_tol == 1e-2 - assert solver.max_iter == 1000 - assert solver.hessian_approximation == "exact" - assert solver.limited_memory_max_history == 50 - assert solver.linear_solver == "mumps" - assert solver.nlp_scaling_method == "gradient-based" - assert solver.mu_init == 0.1 - assert solver.warm_start_init_point == "no" - assert solver.warm_start_mult_bound_push == 0.001 - assert solver.warm_start_slack_bound_push == 0.001 - assert solver.warm_start_bound_push == 0.001 - assert solver.warm_start_slack_bound_frac == 0.001 - assert solver.warm_start_bound_frac == 0.001 - assert solver.bound_push == 0.01 - assert solver.bound_frac == 0.01 - assert solver.print_level == 5 - assert solver.c_compile is False - assert solver.check_derivatives_for_naninf == "no" - - solver.set_linear_solver("ma57") - assert solver.linear_solver == "ma57" - solver.set_tol(2) - assert solver.tol == 2 - solver.set_dual_inf_tol(3) - assert solver.dual_inf_tol == 3 - solver.set_constr_viol_tol(4) - assert solver.constr_viol_tol == 4 - solver.set_compl_inf_tol(5) - assert solver.compl_inf_tol == 5 - solver.set_acceptable_tol(6) - assert solver.acceptable_tol == 6 - solver.set_acceptable_dual_inf_tol(7) - assert solver.acceptable_dual_inf_tol == 7 - solver.set_acceptable_constr_viol_tol(8) - assert solver.acceptable_constr_viol_tol == 8 - solver.set_acceptable_compl_inf_tol(9) - assert solver.acceptable_compl_inf_tol == 9 - solver.set_maximum_iterations(10) - assert solver.max_iter == 10 - solver.set_hessian_approximation("hello bioptim") - assert solver.hessian_approximation == "hello bioptim" - solver.set_nlp_scaling_method("how are you?") - assert solver.nlp_scaling_method == "how are you?" - solver.set_limited_memory_max_history(11) - assert solver.limited_memory_max_history == 11 - solver.set_mu_init(12) - assert solver.mu_init == 12 - solver.set_warm_start_init_point("super!") - assert solver.warm_start_init_point == "super!" - solver.set_warm_start_mult_bound_push(13) - assert solver.warm_start_mult_bound_push == 13 - solver.set_warm_start_slack_bound_push(14) - assert solver.warm_start_slack_bound_push == 14 - solver.set_warm_start_bound_push(15) - assert solver.warm_start_bound_push == 15 - solver.set_warm_start_slack_bound_frac(16) - assert solver.warm_start_slack_bound_frac == 16 - solver.set_warm_start_bound_frac(17) - assert solver.warm_start_bound_frac == 17 - solver.set_bound_push(18) - assert solver.bound_push == 18 - solver.set_bound_frac(19) - assert solver.bound_frac == 19 - solver.set_print_level(20) - assert solver.print_level == 20 - solver.set_c_compile(True) - assert solver.c_compile is True - solver.set_check_derivatives_for_naninf(True) - assert solver.check_derivatives_for_naninf == "yes" - - solver.set_convergence_tolerance(21) - assert solver.tol == 21 - assert solver.acceptable_tol == 21 - assert solver.compl_inf_tol == 21 - assert solver.acceptable_compl_inf_tol == 21 - - solver.set_constraint_tolerance(22) - assert solver.constr_viol_tol == 22 - assert solver.acceptable_constr_viol_tol == 22 - - solver.set_warm_start_options(42) - assert solver.warm_start_init_point == "yes" - assert solver.mu_init == 42 - assert solver.warm_start_mult_bound_push == 42 - assert solver.warm_start_slack_bound_push == 42 - assert solver.warm_start_bound_push == 42 - assert solver.warm_start_slack_bound_frac == 42 - assert solver.warm_start_bound_frac == 42 - - solver.set_initialization_options(44) - assert solver.bound_push == 44 - assert solver.bound_frac == 44 - - solver.set_option_unsafe(666, "mysterious option") - assert solver.__dict__["_mysterious option"] == 666 - - fake_solver = FakeSolver(options_common={"ipopt.casino_gain": 777}) - solver_dict = solver.as_dict(fake_solver) - assert solver_dict["ipopt.casino_gain"] == 777 - assert solver_dict["ipopt.tol"] == 21 - assert not "_c_compile" in solver_dict - assert not "type" in solver_dict - assert not "show_online_optim" in solver_dict - assert not "online_optim" in solver_dict - assert not "show_options" in solver_dict - - solver.set_nlp_scaling_method("gradient-fiesta") - assert solver.nlp_scaling_method == "gradient-fiesta" diff --git a/tests/shard6/test_time_dependent_custom_model.py b/tests/shard4/test_time_dependent_custom_model.py similarity index 100% rename from tests/shard6/test_time_dependent_custom_model.py rename to tests/shard4/test_time_dependent_custom_model.py diff --git a/tests/shard6/test_time_dependent_problems.py b/tests/shard4/test_time_dependent_problems.py similarity index 100% rename from tests/shard6/test_time_dependent_problems.py rename to tests/shard4/test_time_dependent_problems.py diff --git a/tests/shard6/test_configure_problem.py b/tests/shard5/test_configure_problem.py similarity index 100% rename from tests/shard6/test_configure_problem.py rename to tests/shard5/test_configure_problem.py diff --git a/tests/shard1/test_custom_model.py b/tests/shard5/test_custom_model.py similarity index 100% rename from tests/shard1/test_custom_model.py rename to tests/shard5/test_custom_model.py diff --git a/tests/shard1/test_dynamics.py b/tests/shard5/test_dynamics.py similarity index 100% rename from tests/shard1/test_dynamics.py rename to tests/shard5/test_dynamics.py diff --git a/tests/shard1/test_dynamics_units.py b/tests/shard5/test_dynamics_units.py similarity index 100% rename from tests/shard1/test_dynamics_units.py rename to tests/shard5/test_dynamics_units.py diff --git a/tests/shard1/test_global_fatigue.py b/tests/shard5/test_global_fatigue.py similarity index 99% rename from tests/shard1/test_global_fatigue.py rename to tests/shard5/test_global_fatigue.py index ee49d2658..606a70434 100644 --- a/tests/shard1/test_global_fatigue.py +++ b/tests/shard5/test_global_fatigue.py @@ -629,7 +629,7 @@ def test_fatigable_effort_torque_split(phase_dynamics): sol = ocp.solve() # Check objective function value - if platform.system() != "Linux": + if platform.system() == "Darwin": TestUtils.assert_objective_value(sol=sol, expected_value=124.09811263203727) # Check constraints diff --git a/tests/shard2/test_global_minimize_marker_velocity.py b/tests/shard5/test_global_minimize_marker_velocity.py similarity index 100% rename from tests/shard2/test_global_minimize_marker_velocity.py rename to tests/shard5/test_global_minimize_marker_velocity.py diff --git a/tests/shard3/test_global_torque_driven_ocp.py b/tests/shard5/test_global_torque_driven_ocp.py similarity index 100% rename from tests/shard3/test_global_torque_driven_ocp.py rename to tests/shard5/test_global_torque_driven_ocp.py diff --git a/tests/shard3/test_global_torque_driven_with_contact_ocp.py b/tests/shard5/test_global_torque_driven_with_contact_ocp.py similarity index 100% rename from tests/shard3/test_global_torque_driven_with_contact_ocp.py rename to tests/shard5/test_global_torque_driven_with_contact_ocp.py diff --git a/tests/shard3/test_initial_condition.py b/tests/shard5/test_initial_condition.py similarity index 100% rename from tests/shard3/test_initial_condition.py rename to tests/shard5/test_initial_condition.py diff --git a/tests/shard4/test_solution.py b/tests/shard5/test_solution.py similarity index 100% rename from tests/shard4/test_solution.py rename to tests/shard5/test_solution.py diff --git a/tests/shard5/test_solver_options.py b/tests/shard5/test_solver_options.py new file mode 100644 index 000000000..d4cde2e5d --- /dev/null +++ b/tests/shard5/test_solver_options.py @@ -0,0 +1,296 @@ +import pytest + +from bioptim import Solver +from bioptim.gui.online_callback_server import OnlineCallbackServer, _ResponseHeader +from bioptim.gui.online_callback_multiprocess_server import OnlineCallbackMultiprocessServer +from bioptim.interfaces.interface_utils import generic_online_optim +from bioptim.misc.enums import SolverType, OnlineOptim + + +class FakeSolver: + def __init__( + self, + options_common: dict = None, + ): + self.options_common = options_common + + +class FakeInterface: + def __init__(self, online_optim): + self.opts = type("Opts", (), {"online_optim": online_optim})() + self.options_common = {} + + +def test_ipopt_solver_options(): + solver = Solver.IPOPT() + assert solver.type == SolverType.IPOPT + assert solver.show_online_optim is None + assert solver.online_optim is None + assert solver.show_options is None + assert solver.tol == 1e-6 + assert solver.dual_inf_tol == 1.0 + assert solver.constr_viol_tol == 0.0001 + assert solver.compl_inf_tol == 0.0001 + assert solver.acceptable_tol == 1e-6 + assert solver.acceptable_dual_inf_tol == 1e10 + assert solver.acceptable_constr_viol_tol == 1e-2 + assert solver.acceptable_compl_inf_tol == 1e-2 + assert solver.max_iter == 1000 + assert solver.hessian_approximation == "exact" + assert solver.limited_memory_max_history == 50 + assert solver.linear_solver == "mumps" + assert solver.nlp_scaling_method == "gradient-based" + assert solver.mu_init == 0.1 + assert solver.warm_start_init_point == "no" + assert solver.warm_start_mult_bound_push == 0.001 + assert solver.warm_start_slack_bound_push == 0.001 + assert solver.warm_start_bound_push == 0.001 + assert solver.warm_start_slack_bound_frac == 0.001 + assert solver.warm_start_bound_frac == 0.001 + assert solver.bound_push == 0.01 + assert solver.bound_frac == 0.01 + assert solver.print_level == 5 + assert solver.c_compile is False + assert solver.check_derivatives_for_naninf == "no" + + solver.set_linear_solver("ma57") + assert solver.linear_solver == "ma57" + solver.set_tol(2) + assert solver.tol == 2 + solver.set_dual_inf_tol(3) + assert solver.dual_inf_tol == 3 + solver.set_constr_viol_tol(4) + assert solver.constr_viol_tol == 4 + solver.set_compl_inf_tol(5) + assert solver.compl_inf_tol == 5 + solver.set_acceptable_tol(6) + assert solver.acceptable_tol == 6 + solver.set_acceptable_dual_inf_tol(7) + assert solver.acceptable_dual_inf_tol == 7 + solver.set_acceptable_constr_viol_tol(8) + assert solver.acceptable_constr_viol_tol == 8 + solver.set_acceptable_compl_inf_tol(9) + assert solver.acceptable_compl_inf_tol == 9 + solver.set_maximum_iterations(10) + assert solver.max_iter == 10 + solver.set_hessian_approximation("hello bioptim") + assert solver.hessian_approximation == "hello bioptim" + solver.set_nlp_scaling_method("how are you?") + assert solver.nlp_scaling_method == "how are you?" + solver.set_limited_memory_max_history(11) + assert solver.limited_memory_max_history == 11 + solver.set_mu_init(12) + assert solver.mu_init == 12 + solver.set_warm_start_init_point("super!") + assert solver.warm_start_init_point == "super!" + solver.set_warm_start_mult_bound_push(13) + assert solver.warm_start_mult_bound_push == 13 + solver.set_warm_start_slack_bound_push(14) + assert solver.warm_start_slack_bound_push == 14 + solver.set_warm_start_bound_push(15) + assert solver.warm_start_bound_push == 15 + solver.set_warm_start_slack_bound_frac(16) + assert solver.warm_start_slack_bound_frac == 16 + solver.set_warm_start_bound_frac(17) + assert solver.warm_start_bound_frac == 17 + solver.set_bound_push(18) + assert solver.bound_push == 18 + solver.set_bound_frac(19) + assert solver.bound_frac == 19 + solver.set_print_level(20) + assert solver.print_level == 20 + solver.set_c_compile(True) + assert solver.c_compile is True + solver.set_check_derivatives_for_naninf(True) + assert solver.check_derivatives_for_naninf == "yes" + + solver.set_convergence_tolerance(21) + assert solver.tol == 21 + assert solver.acceptable_tol == 21 + assert solver.compl_inf_tol == 21 + assert solver.acceptable_compl_inf_tol == 21 + + solver.set_constraint_tolerance(22) + assert solver.constr_viol_tol == 22 + assert solver.acceptable_constr_viol_tol == 22 + + solver.set_warm_start_options(42) + assert solver.warm_start_init_point == "yes" + assert solver.mu_init == 42 + assert solver.warm_start_mult_bound_push == 42 + assert solver.warm_start_slack_bound_push == 42 + assert solver.warm_start_bound_push == 42 + assert solver.warm_start_slack_bound_frac == 42 + assert solver.warm_start_bound_frac == 42 + + solver.set_initialization_options(44) + assert solver.bound_push == 44 + assert solver.bound_frac == 44 + + solver.set_option_unsafe(666, "mysterious option") + assert solver.__dict__["_mysterious option"] == 666 + + fake_solver = FakeSolver(options_common={"ipopt.casino_gain": 777}) + solver_dict = solver.as_dict(fake_solver) + assert solver_dict["ipopt.casino_gain"] == 777 + assert solver_dict["ipopt.tol"] == 21 + assert not "_c_compile" in solver_dict + assert not "type" in solver_dict + assert not "show_online_optim" in solver_dict + assert not "online_optim" in solver_dict + assert not "show_options" in solver_dict + + solver.set_nlp_scaling_method("gradient-fiesta") + assert solver.nlp_scaling_method == "gradient-fiesta" + + +def test_generic_online_optim_skips_when_default_is_unavailable(monkeypatch): + interface = FakeInterface(OnlineOptim.DEFAULT) + + monkeypatch.setattr(OnlineOptim, "get_default", lambda self: None) + + generic_online_optim(interface, ocp=None) + + assert interface.options_common == {} + + +def test_generic_online_optim_uses_multiprocess_on_linux(monkeypatch): + interface = FakeInterface(OnlineOptim.DEFAULT) + callback = object() + + monkeypatch.setattr("bioptim.misc.enums.platform.system", lambda: "Linux") + monkeypatch.setattr( + "bioptim.interfaces.interface_utils.OnlineCallbackMultiprocessServer", lambda *args, **kwargs: callback + ) + + generic_online_optim(interface, ocp=None) + + assert interface.options_common["iteration_callback"] is callback + + +def test_generic_online_optim_uses_multiprocess_server_on_windows(monkeypatch): + interface = FakeInterface(OnlineOptim.DEFAULT) + callback = object() + + monkeypatch.setattr("bioptim.misc.enums.platform.system", lambda: "Windows") + monkeypatch.setattr( + "bioptim.interfaces.interface_utils.OnlineCallbackMultiprocessServer", lambda *args, **kwargs: callback + ) + + generic_online_optim(interface, ocp=None) + + assert interface.options_common["iteration_callback"] is callback + + +def test_generic_online_optim_uses_multiprocess_server_on_macos(monkeypatch): + interface = FakeInterface(OnlineOptim.DEFAULT) + callback = object() + + monkeypatch.setattr("bioptim.misc.enums.platform.system", lambda: "Darwin") + monkeypatch.setattr( + "bioptim.interfaces.interface_utils.OnlineCallbackMultiprocessServer", lambda *args, **kwargs: callback + ) + + generic_online_optim(interface, ocp=None) + + assert interface.options_common["iteration_callback"] is callback + + +def test_generic_online_optim_rejects_multiprocess_on_macos(monkeypatch): + interface = FakeInterface(OnlineOptim.MULTIPROCESS) + + monkeypatch.setattr("bioptim.interfaces.interface_utils.platform.system", lambda: "Darwin") + + with pytest.raises(NotImplementedError, match="MULTIPROCESS is not available on macOS"): + generic_online_optim(interface, ocp=None) + + +def test_generic_online_optim_allows_server_on_macos(monkeypatch): + interface = FakeInterface(OnlineOptim.SERVER) + callback = object() + + monkeypatch.setattr("bioptim.interfaces.interface_utils.platform.system", lambda: "Darwin") + monkeypatch.setattr("bioptim.interfaces.interface_utils.OnlineCallbackServer", lambda *args, **kwargs: callback) + + generic_online_optim(interface, ocp=None) + + assert interface.options_common["iteration_callback"] is callback + + +def test_online_callback_server_recreates_socket_between_retries(monkeypatch): + class FakeOcp: + n_phases = 1 + + class FakeSocket: + def __init__(self, should_fail=False): + self.should_fail = should_fail + self.closed = False + self.sent = [] + + def connect(self, _): + if self.should_fail: + raise ConnectionRefusedError("server not ready") + + def close(self): + self.closed = True + + def sendall(self, data): + self.sent.append(data) + + def recv(self, _): + return _ResponseHeader.PLOT_READY.encode() + + sockets = [FakeSocket(should_fail=True), FakeSocket(should_fail=False)] + + monkeypatch.setattr("bioptim.gui.online_callback_server.socket.socket", lambda *args, **kwargs: sockets.pop(0)) + monkeypatch.setattr("bioptim.gui.online_callback_server.time.sleep", lambda *_: None) + monkeypatch.setattr( + "bioptim.gui.online_callback_server.OcpSerializable.from_ocp", + lambda ocp: type("FakeSerializable", (), {"serialize": lambda self: {}})(), + ) + monkeypatch.setattr( + "bioptim.gui.online_callback_server.OptimizationVectorHelper.extract_step_times", lambda ocp, _: [] + ) + monkeypatch.setattr("bioptim.gui.online_callback_server.PlotOcp", lambda *args, **kwargs: "plotter") + + callback = OnlineCallbackServer.__new__(OnlineCallbackServer) + callback.ocp = FakeOcp() + callback._host = "localhost" + callback._port = 3050 + callback._socket = None + callback._should_wait_ok_to_client_on_new_data = False + callback._has_received_ok = lambda: True + + callback._reset_client_socket() + first_socket = callback._socket + + callback._initialize_connexion() + + assert first_socket.closed is True + assert callback._socket.sent + assert callback._plotter == "plotter" + + +def test_online_callback_multiprocess_server_uses_free_port_when_missing(monkeypatch): + recorded = {} + + class FakeProcess: + def __init__(self, target, kwargs): + recorded["process_target"] = target + recorded["process_kwargs"] = kwargs + + def start(self): + recorded["started"] = True + + def fake_super_init(self, *args, **kwargs): + recorded["super_kwargs"] = kwargs + + monkeypatch.setattr("bioptim.gui.online_callback_multiprocess_server._find_available_tcp_port", lambda host: 4242) + monkeypatch.setattr("bioptim.gui.online_callback_multiprocess_server.Process", FakeProcess) + monkeypatch.setattr(OnlineCallbackServer, "__init__", fake_super_init) + + OnlineCallbackMultiprocessServer(ocp=None) + + assert recorded["process_kwargs"]["port"] == 4242 + assert recorded["super_kwargs"]["port"] == 4242 + assert recorded["started"] is True diff --git a/tests/shard1/test_bounds_accessor.py b/tests/shard6/test_bounds_accessor.py similarity index 100% rename from tests/shard1/test_bounds_accessor.py rename to tests/shard6/test_bounds_accessor.py diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard6/test_continuity_as_objective.py similarity index 100% rename from tests/shard1/test_continuity_as_objective.py rename to tests/shard6/test_continuity_as_objective.py diff --git a/tests/shard1/test_continuity_linear_continuous.py b/tests/shard6/test_continuity_linear_continuous.py similarity index 100% rename from tests/shard1/test_continuity_linear_continuous.py rename to tests/shard6/test_continuity_linear_continuous.py diff --git a/tests/shard1/test_controltype_none.py b/tests/shard6/test_controltype_none.py similarity index 100% rename from tests/shard1/test_controltype_none.py rename to tests/shard6/test_controltype_none.py diff --git a/tests/shard1/test_defects.py b/tests/shard6/test_defects.py similarity index 100% rename from tests/shard1/test_defects.py rename to tests/shard6/test_defects.py diff --git a/tests/shard1/test_enums.py b/tests/shard6/test_enums.py similarity index 100% rename from tests/shard1/test_enums.py rename to tests/shard6/test_enums.py diff --git a/tests/shard3/test_get_time_solution.py b/tests/shard6/test_get_time_solution.py similarity index 100% rename from tests/shard3/test_get_time_solution.py rename to tests/shard6/test_get_time_solution.py diff --git a/tests/shard1/test_global_align.py b/tests/shard6/test_global_align.py similarity index 100% rename from tests/shard1/test_global_align.py rename to tests/shard6/test_global_align.py diff --git a/tests/shard1/test_global_fail.py b/tests/shard6/test_global_fail.py similarity index 100% rename from tests/shard1/test_global_fail.py rename to tests/shard6/test_global_fail.py diff --git a/tests/shard2/test_global_inverse_optimal_control.py b/tests/shard6/test_global_inverse_optimal_control.py similarity index 100% rename from tests/shard2/test_global_inverse_optimal_control.py rename to tests/shard6/test_global_inverse_optimal_control.py diff --git a/tests/shard2/test_global_muscle_driven_ocp.py b/tests/shard6/test_global_muscle_driven_ocp.py similarity index 100% rename from tests/shard2/test_global_muscle_driven_ocp.py rename to tests/shard6/test_global_muscle_driven_ocp.py diff --git a/tests/shard2/test_global_muscle_driven_ocp_with_contacts.py b/tests/shard6/test_global_muscle_driven_ocp_with_contacts.py similarity index 100% rename from tests/shard2/test_global_muscle_driven_ocp_with_contacts.py rename to tests/shard6/test_global_muscle_driven_ocp_with_contacts.py diff --git a/tests/shard2/test_global_muscle_tracking_0_False.py b/tests/shard6/test_global_muscle_tracking_0_False.py similarity index 100% rename from tests/shard2/test_global_muscle_tracking_0_False.py rename to tests/shard6/test_global_muscle_tracking_0_False.py diff --git a/tests/shard2/test_global_muscle_tracking_0_True.py b/tests/shard6/test_global_muscle_tracking_0_True.py similarity index 100% rename from tests/shard2/test_global_muscle_tracking_0_True.py rename to tests/shard6/test_global_muscle_tracking_0_True.py diff --git a/tests/shard2/test_global_muscle_tracking_1.py b/tests/shard6/test_global_muscle_tracking_1.py similarity index 100% rename from tests/shard2/test_global_muscle_tracking_1.py rename to tests/shard6/test_global_muscle_tracking_1.py diff --git a/tests/shard2/test_global_muscle_tracking_2.py b/tests/shard6/test_global_muscle_tracking_2.py similarity index 100% rename from tests/shard2/test_global_muscle_tracking_2.py rename to tests/shard6/test_global_muscle_tracking_2.py diff --git a/tests/shard2/test_global_optimal_time.py b/tests/shard6/test_global_optimal_time.py similarity index 100% rename from tests/shard2/test_global_optimal_time.py rename to tests/shard6/test_global_optimal_time.py diff --git a/tests/shard2/test_global_optimal_time_mayer_min.py b/tests/shard6/test_global_optimal_time_mayer_min.py similarity index 100% rename from tests/shard2/test_global_optimal_time_mayer_min.py rename to tests/shard6/test_global_optimal_time_mayer_min.py diff --git a/tests/shard2/test_global_sqp.py b/tests/shard6/test_global_sqp.py similarity index 100% rename from tests/shard2/test_global_sqp.py rename to tests/shard6/test_global_sqp.py diff --git a/tests/shard3/test_global_symmetrical_torque_driven_ocp.py b/tests/shard6/test_global_symmetrical_torque_driven_ocp.py similarity index 100% rename from tests/shard3/test_global_symmetrical_torque_driven_ocp.py rename to tests/shard6/test_global_symmetrical_torque_driven_ocp.py diff --git a/tests/shard5/test_helper_socp.py b/tests/shard6/test_helper_socp.py similarity index 100% rename from tests/shard5/test_helper_socp.py rename to tests/shard6/test_helper_socp.py diff --git a/tests/shard3/test_ligaments.py b/tests/shard6/test_ligaments.py similarity index 100% rename from tests/shard3/test_ligaments.py rename to tests/shard6/test_ligaments.py diff --git a/tests/shard3/test_mappings.py b/tests/shard6/test_mappings.py similarity index 100% rename from tests/shard3/test_mappings.py rename to tests/shard6/test_mappings.py diff --git a/tests/shard3/test_multinode_constraints.py b/tests/shard6/test_multinode_constraints.py similarity index 100% rename from tests/shard3/test_multinode_constraints.py rename to tests/shard6/test_multinode_constraints.py diff --git a/tests/shard3/test_multiphase_noised_initial_guess.py b/tests/shard6/test_multiphase_noised_initial_guess.py similarity index 100% rename from tests/shard3/test_multiphase_noised_initial_guess.py rename to tests/shard6/test_multiphase_noised_initial_guess.py diff --git a/tests/shard6/test_param_obj_and_scaling.py b/tests/shard6/test_param_obj_and_scaling.py index f8e94d96b..e3041ca47 100644 --- a/tests/shard6/test_param_obj_and_scaling.py +++ b/tests/shard6/test_param_obj_and_scaling.py @@ -11,34 +11,18 @@ from ..utils import TestUtils -@pytest.mark.parametrize( - "phase_dynamics", - [ - PhaseDynamics.SHARED_DURING_THE_PHASE, - PhaseDynamics.ONE_PER_NODE, - ], -) -@pytest.mark.parametrize( - "use_sx", - [ - True, - False, - ], -) +@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) +@pytest.mark.parametrize("use_sx", [True, False]) def test_example_param_obj_and_param_scaling( phase_dynamics, use_sx, ): - - if platform == "darwin" or platform == "win32": - pytest.skip("This test is not working on MacOS or Windows") - from bioptim.examples.toy_examples.feature_examples import example_parameter_scaling as ocp_module bioptim_folder = TestUtils.bioptim_folder() final_time = 1 - n_shooting = 10 + n_shooting = 20 ocp_to_track = ocp_module.generate_dat_to_track( biorbd_model_path=bioptim_folder + "/examples/models/pendulum_wrong_gravity.bioMod", @@ -67,7 +51,7 @@ def test_example_param_obj_and_param_scaling( sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) # Test the objective values - TestUtils.assert_objective_value(sol=sol, expected_value=35328792.2512389, decimal=4) + TestUtils.assert_objective_value(sol=sol, expected_value=35479.23885709513, decimal=4) g = np.array(sol.constraints) npt.assert_equal(g.shape, (0, 1)) @@ -81,11 +65,11 @@ def test_example_param_obj_and_param_scaling( npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0]), decimal=5) npt.assert_almost_equal(q[:, 0], np.array([0.0, 0.0]), decimal=5) - npt.assert_almost_equal(q[:, 5], np.array([-0.26673, 2.53154]), decimal=5) + npt.assert_almost_equal(q[:, 5], np.array([-0.27469, 0.24421]), decimal=5) npt.assert_almost_equal(q[:, -1], np.array([0.0, 3.14]), decimal=5) param_not_scaled = sol.decision_parameters(scaled=False) param_scaled = sol.decision_parameters(scaled=True) - npt.assert_almost_equal(param_not_scaled["gravity_xyz"], np.array([0.0, 3.89005766, -14.71310026]), decimal=5) - npt.assert_almost_equal(param_scaled["gravity_xyz"], np.array([0.0, 3.89005766, -1.47131003]), decimal=5) + npt.assert_almost_equal(param_not_scaled["gravity_xyz"], np.array([0.0, 0.01281, -5]), decimal=5) + npt.assert_almost_equal(param_scaled["gravity_xyz"], np.array([0.0, 0.01281, -0.5]), decimal=5) diff --git a/tests/shard3/test_parameters.py b/tests/shard6/test_parameters.py similarity index 100% rename from tests/shard3/test_parameters.py rename to tests/shard6/test_parameters.py diff --git a/tests/shard4/test_penalty.py b/tests/shard6/test_penalty.py similarity index 100% rename from tests/shard4/test_penalty.py rename to tests/shard6/test_penalty.py diff --git a/tests/shard6/test_pinocchio_model.py b/tests/shard6/test_pinocchio_model.py new file mode 100644 index 000000000..0887090d4 --- /dev/null +++ b/tests/shard6/test_pinocchio_model.py @@ -0,0 +1,158 @@ +import importlib.util + +import numpy as np +import pytest + +from bioptim import PinocchioModel +from ..utils import TestUtils + +HAS_PINOCCHIO = importlib.util.find_spec("pinocchio") is not None + + +def test_pinocchio_model_import_is_lazy(): + with pytest.raises(ValueError, match="The model should be of type 'str' or 'pinocchio.Model'"): + PinocchioModel(1) + + with pytest.raises(ValueError, match="The model should be of type 'str' or 'pinocchio.Model'"): + PinocchioModel([]) + + +def test_pinocchio_missing_dependency_message(): + if HAS_PINOCCHIO: + pytest.skip("Pinocchio is installed in this environment") + + with pytest.raises(ModuleNotFoundError, match="requires the optional dependency 'pinocchio'"): + PinocchioModel("model.urdf") + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_model_from_urdf_has_expected_basic_properties(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + assert model.nb_q > 0 + assert model.nb_qdot > 0 + assert model.nb_tau > 0 + assert model.nb_segments > 0 + assert model.nb_root == 0 + assert model.nb_quaternions >= 0 + + segment_name = list(model._model.names)[1] + assert model.segment_index(segment_name) == 1 + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_segment_index_raises_for_unknown_segment(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + with pytest.raises(ValueError, match="not_a_segment is not a segment name"): + model.segment_index("not_a_segment") + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_gravity_setter_updates_gravity_function_value(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + model.set_gravity(np.array([0.1, -0.2, -3.5])) + updated_gravity = np.array(model._model.gravity.linear).squeeze() + np.testing.assert_almost_equal(updated_gravity, np.array([0.1, -0.2, -3.5])) + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_function_caching_for_center_of_mass_and_marker(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + assert len(model._cached_functions.keys()) == 0 + + model.center_of_mass()(model.q, model.parameters) + com_id = id(model._cached_functions[("center_of_mass", (), frozenset())]) + assert len(model._cached_functions.keys()) == 1 + + model.center_of_mass()(model.q, model.parameters) + assert com_id == id(model._cached_functions[("center_of_mass", (), frozenset())]) + assert len(model._cached_functions.keys()) == 1 + + if model.nb_markers > 0: + model.marker(index=0)(model.q, model.parameters) + marker_id = id(model._cached_functions[("marker", (), frozenset({("index", 0)}))]) + assert len(model._cached_functions.keys()) == 2 + + model.marker(index=0)(model.q, model.parameters) + assert marker_id == id(model._cached_functions[("marker", (), frozenset({("index", 0)}))]) + assert len(model._cached_functions.keys()) == 2 + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_not_implemented_methods_raise(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + with pytest.raises(NotImplementedError, match="forward_dynamics with contact is not implemented"): + model.forward_dynamics(with_contact=True) + + with pytest.raises(NotImplementedError, match="Animation is not implemented"): + model.animate() + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_copy_and_serialize_from_path_model(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + model_copy = model.copy() + assert isinstance(model_copy, PinocchioModel) + assert model_copy is not model + assert model_copy.path == model_path + + serializer, data = model.serialize() + assert serializer is PinocchioModel + assert data["bio_model"] == model_path + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_ranges_and_marker_index_error_paths(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + q_ranges = model.ranges_from_model("q") + qdot_ranges = model.ranges_from_model("qdot") + qddot_ranges = model.ranges_from_model("qddot") + assert len(q_ranges) == model.nb_q + assert len(qdot_ranges) == model.nb_qdot + assert len(qddot_ranges) == model.nb_qddot + + with pytest.raises(RuntimeError, match="Wrong variable name"): + model.ranges_from_model("invalid") + + with pytest.raises(ValueError, match="not_a_marker is not a marker name"): + model.marker_index("not_a_marker") + + +@pytest.mark.skipif(not HAS_PINOCCHIO, reason="Pinocchio is not installed") +def test_pinocchio_forward_and_inverse_dynamics_shapes(): + bioptim_folder = TestUtils.bioptim_folder() + model_path = bioptim_folder + "/examples/models/pendulum.urdf" + model = PinocchioModel(model_path) + + q = np.zeros((model.nb_q, 1)) + qdot = np.zeros((model.nb_qdot, 1)) + tau = np.zeros((model.nb_tau, 1)) + qddot = np.zeros((model.nb_qddot, 1)) + external_forces = np.zeros((0, 1)) + parameters = np.zeros((0, 1)) + + fd = model.forward_dynamics()(q, qdot, tau, external_forces, parameters) + id_tau = model.inverse_dynamics()(q, qdot, qddot, external_forces, parameters) + + np.testing.assert_equal(np.array(fd).shape, (model.nb_qddot, 1)) + np.testing.assert_equal(np.array(id_tau).shape, (model.nb_tau, 1)) diff --git a/tests/shard4/test_update_bounds_and_init.py b/tests/shard6/test_update_bounds_and_init.py similarity index 100% rename from tests/shard4/test_update_bounds_and_init.py rename to tests/shard6/test_update_bounds_and_init.py