From 1fc164e6956cd78293179f9c02fc4ee2a3c66001 Mon Sep 17 00:00:00 2001 From: Martin Lacaille Date: Fri, 19 Jan 2024 14:31:59 -0500 Subject: [PATCH 01/12] FIX: example which had an unexpected behavior, and a new test that the new implementation should pass --- .../example_continuity_as_objective.py | 158 ++++++++++++++---- tests/shard1/test_continuity_as_objective.py | 152 ++++++++++++++++- 2 files changed, 275 insertions(+), 35 deletions(-) diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index a762d28ee..b527ed062 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -37,7 +37,6 @@ Solution, PenaltyController, PhaseDynamics, - SolutionMerge, ) @@ -137,14 +136,61 @@ def prepare_ocp_first_pass( u_init.add_noise(bounds=u_bounds, magnitude=0.01, n_shooting=n_shooting) constraints = ConstraintList() - constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="marker_2", second_marker="target_2") - constraints.add(out_of_sphere, y=-0.45, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=0.05, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + node=Node.END, + first_marker="marker_2", + second_marker="target_2", + ) + constraints.add( + out_of_sphere, + y=-0.45, + z=0, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=0.05, + z=0, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) # for another good example, comment out this line below here and in second pass (see HERE) - constraints.add(out_of_sphere, y=0.55, z=-0.85, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=0.75, z=0.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=1.4, z=0.5, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=2, z=1.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add( + out_of_sphere, + y=0.55, + z=-0.85, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=0.75, + z=0.2, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=1.4, + z=0.5, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=2, + z=1.2, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) return OptimalControlProgram( bio_model, @@ -165,7 +211,6 @@ def prepare_ocp_first_pass( def prepare_ocp_second_pass( biorbd_model_path: str, - ns: int, solution: Solution, ode_solver: OdeSolverBase = OdeSolver.RK4(), use_sx: bool = True, @@ -208,10 +253,11 @@ def prepare_ocp_second_pass( x_bounds["qdot"][:, 0] = 0 # Initial guess - states = solution.decision_states(to_merge=SolutionMerge.NODES) x_init = InitialGuessList() - x_init.add("q", states["q"], interpolation=InterpolationType.EACH_FRAME) - x_init.add("qdot", states["qdot"], interpolation=InterpolationType.EACH_FRAME) + x_init.add("q", solution.states["q"], interpolation=InterpolationType.EACH_FRAME) + x_init.add( + "qdot", solution.states["qdot"], interpolation=InterpolationType.EACH_FRAME + ) # Define control path constraint n_tau = bio_model.nb_tau @@ -220,26 +266,75 @@ def prepare_ocp_second_pass( u_bounds["tau"] = [tau_min] * n_tau, [tau_max] * n_tau u_bounds["tau"][1, :] = 0 # Prevent the model from actively rotate - controls = solution.decision_controls(to_merge=SolutionMerge.NODES) u_init = InitialGuessList() - u_init.add("tau", controls["tau"], interpolation=InterpolationType.EACH_FRAME) + u_init.add( + "tau", + solution.controls["tau"][:, :-1], + interpolation=InterpolationType.EACH_FRAME, + ) constraints = ConstraintList() - constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="marker_2", second_marker="target_2") - constraints.add(out_of_sphere, y=-0.45, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=0.05, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add( + ConstraintFcn.SUPERIMPOSE_MARKERS, + node=Node.END, + first_marker="marker_2", + second_marker="target_2", + ) + constraints.add( + out_of_sphere, + y=-0.45, + z=0, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=0.05, + z=0, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) # HERE (referenced in first pass) - constraints.add(out_of_sphere, y=0.55, z=-0.85, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=0.75, z=0.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=1.4, z=0.5, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) - constraints.add(out_of_sphere, y=2, z=1.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add( + out_of_sphere, + y=0.55, + z=-0.85, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=0.75, + z=0.2, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=1.4, + z=0.5, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) + constraints.add( + out_of_sphere, + y=2, + z=1.2, + min_bound=0.35, + max_bound=np.inf, + node=Node.ALL_SHOOTING, + ) - final_time = float(solution.decision_time(to_merge=SolutionMerge.NODES)[-1, 0]) return OptimalControlProgram( bio_model, dynamics, - ns, - final_time, + solution.ns, + solution.phase_time[-1], x_init=x_init, u_init=u_init, x_bounds=x_bounds, @@ -260,11 +355,10 @@ def main(): # --- First pass --- # # --- Prepare the ocp --- # np.random.seed(123456) - n_shooting = 500 ocp_first = prepare_ocp_first_pass( biorbd_model_path="models/pendulum_maze.bioMod", final_time=5, - n_shooting=n_shooting, + n_shooting=500, # change the weight to observe the impact on the continuity of the solution # or comment to see how the constrained program would fare state_continuity_weight=1_000_000, @@ -272,7 +366,10 @@ def main(): ) # ocp_first.print(to_console=True) - solver_first = Solver.IPOPT(show_online_optim=platform.system() == "Linux", show_options=dict(show_bounds=True)) + solver_first = Solver.IPOPT( + show_online_optim=platform.system() == "Linux", + show_options=dict(show_bounds=True), + ) # change maximum iterations to affect the initial solution # it doesn't mather if it exits before the optimal solution, only that there is an initial guess solver_first.set_maximum_iterations(500) @@ -286,11 +383,14 @@ def main(): # # --- Second pass ---# # # --- Prepare the ocp --- # - solver_second = Solver.IPOPT(show_online_optim=platform.system() == "Linux", show_options=dict(show_bounds=True)) + solver_second = Solver.IPOPT( + show_online_optim=platform.system() == "Linux", + show_options=dict(show_bounds=True), + ) solver_second.set_maximum_iterations(10000) ocp_second = prepare_ocp_second_pass( - biorbd_model_path="models/pendulum_maze.bioMod", ns=n_shooting, solution=sol_first, n_threads=3 + biorbd_model_path="models/pendulum_maze.bioMod", solution=sol_first, n_threads=3 ) # Custom plots diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index 7dc22cbf8..e32277af4 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -1,19 +1,29 @@ -from bioptim import PhaseDynamics, SolutionMerge +from bioptim import PhaseDynamics import numpy as np import pytest from tests.utils import TestUtils -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) +@pytest.mark.parametrize( + "phase_dynamics", + [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE], +) def test_continuity_as_objective(phase_dynamics): - from bioptim.examples.getting_started import example_continuity_as_objective as ocp_module + from bioptim.examples.getting_started import ( + example_continuity_as_objective as ocp_module, + ) np.random.seed(42) - model_path = TestUtils.bioptim_folder() + "/examples/getting_started/models/pendulum_maze.bioMod" + model_path = ( + TestUtils.bioptim_folder() + + "/examples/getting_started/models/pendulum_maze.bioMod" + ) + # first pass ocp = ocp_module.prepare_ocp_first_pass( biorbd_model_path=model_path, + n_threads=1, final_time=1, n_shooting=3, state_continuity_weight=1000000, @@ -22,5 +32,135 @@ def test_continuity_as_objective(phase_dynamics): ) sol = ocp.solve() - expected = np.array([-0.1376, 2.9976372]) - np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["q"][:, -1], expected) + # check q in integrality, qdot, controls, iterations + + expected_q = [ + [0.0, -1.1923517, -1.42486622, -0.1376], + [0.0, 1.16898343, 1.88747861, 2.9976372], + ] + + expected_qdot = [ + [0.0, -5.43509473, 2.40522723, 24.88229668], + [0.0, 5.15109901, 3.59555194, 21.98924882], + ] + + expected_controls = [ + [-20.54589655, 26.78617256, 15.76500633, np.nan], + [0.0, 0.0, 0.0, np.nan], + ] + + expected_iterations = [ + 212, + 224, + ] # while the number of iterations is reproductible, it differs between debug and raw runs + np.testing.assert_almost_equal(sol.states["q"], expected_q) + np.testing.assert_almost_equal(sol.states["qdot"], expected_qdot) + np.testing.assert_almost_equal(sol.controls["tau"], expected_controls) + assert sol.iterations in expected_iterations + # second pass + ocp_second_pass = ocp_module.prepare_ocp_second_pass( + biorbd_model_path=model_path, n_threads=1, solution=sol + ) + sol_second_pass = ocp_second_pass.solve() + # check q in integrality, qdot,controls, vector, cost, iterations + + expected_q = [ + [0.0, -1.19252768, -1.42504253, -0.1376], + [0.0, 1.16885905, 1.88734408, 2.9976372], + ] + expected_qdot = [ + [0.0, -5.43603845, 2.40642094, 24.89415277], + [0.0, 5.15237646, 3.59628495, 21.99866436], + ] + expected_vector = [ + [ + 0, + 0, + 0, + 0, + -1.19253, + 1.16886, + -5.43604, + 5.15238, + -1.42504, + 1.88734, + 2.40642, + 3.59628, + -0.1376, + 2.99764, + 24.8942, + 21.9987, + -20.5528, + 0, + 26.7991, + 0, + 15.7774, + 0, + 0.553202, + ] + ] + + expected_constraints = [ + [ + -9.10383e-15, + 5.10703e-15, + 6.21725e-15, + 8.88178e-16, + 5.10703e-15, + -5.9952e-15, + 4.26326e-14, + 2.08722e-14, + 3.91354e-15, + 1.15463e-14, + 8.52651e-14, + 9.9476e-14, + 0, + -3.20577e-15, + -1.4988e-14, + 1.08612, + 0.35, + 0.37334, + 0.954385, + 0.458212, + 0.692135, + 0.492353, + 0.996479, + 1.63013, + 1.34023, + 1.15602, + 1.29985, + 1.97149, + 1.87178, + 1.94454, + 2.89311, + 2.74089, + 2.67422, + ] + ] + + expected_cost = 311.551 + + expected_detailed_cost = [ + { + "cost_value": 22.02370080865957, + "cost_value_weighted": 256.23075256985715, + "name": "Lagrange.MINIMIZE_CONTROL", + }, + { + "cost_value": 0.5532019998370968, + "cost_value_weighted": 55.320199983709685, + "name": "Mayer.MINIMIZE_TIME", + }, + ] + + expected_iterations = [8] # run it TBD + np.testing.assert_almost_equal(sol_second_pass.states["q"], expected_q) + np.testing.assert_almost_equal(sol_second_pass.states["qdot"], expected_qdot) + np.testing.assert_almost_equal(sol_second_pass.vector.T, expected_vector, decimal=1) + np.testing.assert_almost_equal( + sol_second_pass.constraints.T, expected_constraints, decimal=1 + ) + np.testing.assert_almost_equal( + float(sol_second_pass.cost), expected_cost, decimal=2 + ) + assert sol_second_pass.iterations in expected_iterations From 5fbd26ac50ff5b791dddb9935edf3c207dd7db25 Mon Sep 17 00:00:00 2001 From: Martin Lacaille Date: Fri, 19 Jan 2024 16:29:11 -0500 Subject: [PATCH 02/12] FIX: bisouuuuuu --- .../example_continuity_as_objective.py | 11 +- tests/shard1/test_continuity_as_objective.py | 214 +++++++++--------- 2 files changed, 112 insertions(+), 113 deletions(-) diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index b527ed062..ade940231 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -37,6 +37,7 @@ Solution, PenaltyController, PhaseDynamics, + SolutionMerge, ) @@ -131,8 +132,9 @@ def prepare_ocp_first_pass( u_bounds["tau"] = [tau_min] * n_tau, [tau_max] * n_tau u_bounds["tau"][1, :] = 0 # Prevent the model from actively rotate + controls = solution.decision_controls(to_merge=SolutionMerge.NODES) u_init = InitialGuessList() - u_init["tau"] = [tau_init] * n_tau + u_init.add("tau", controls["tau"], interpolation=InterpolationType.EACH_FRAME) u_init.add_noise(bounds=u_bounds, magnitude=0.01, n_shooting=n_shooting) constraints = ConstraintList() @@ -253,11 +255,10 @@ def prepare_ocp_second_pass( x_bounds["qdot"][:, 0] = 0 # Initial guess + states = solution.decision_states(to_merge=SolutionMerge.NODES) x_init = InitialGuessList() - x_init.add("q", solution.states["q"], interpolation=InterpolationType.EACH_FRAME) - x_init.add( - "qdot", solution.states["qdot"], interpolation=InterpolationType.EACH_FRAME - ) + x_init.add("q", states["q"], interpolation=InterpolationType.EACH_FRAME) + x_init.add("qdot", states["qdot"], interpolation=InterpolationType.EACH_FRAME) # Define control path constraint n_tau = bio_model.nb_tau diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index e32277af4..716e911ae 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -32,31 +32,29 @@ def test_continuity_as_objective(phase_dynamics): ) sol = ocp.solve() - # check q in integrality, qdot, controls, iterations + # # check q in integrality, qdot, controls, iterations + # + # expected_q = [ + # [0.0, -1.1923517, -1.42486622, -0.1376], + # [0.0, 1.16898343, 1.88747861, 2.9976372], + # ] + # + # expected_qdot = [ + # [0.0, -5.43509473, 2.40522723, 24.88229668], + # [0.0, 5.15109901, 3.59555194, 21.98924882], + # ] + # + # expected_controls = [ + # [-20.54589655, 26.78617256, 15.76500633, np.nan], + # [0.0, 0.0, 0.0, np.nan], + # ] + # + # expected_iterations = range(200, 250) # while the number of iterations is reproductible, it differs between debug and raw runs + # np.testing.assert_almost_equal(sol.states["q"], expected_q) + # np.testing.assert_almost_equal(sol.states["qdot"], expected_qdot) + # np.testing.assert_almost_equal(sol.controls["tau"], expected_controls) + # assert sol.iterations in expected_iterations - expected_q = [ - [0.0, -1.1923517, -1.42486622, -0.1376], - [0.0, 1.16898343, 1.88747861, 2.9976372], - ] - - expected_qdot = [ - [0.0, -5.43509473, 2.40522723, 24.88229668], - [0.0, 5.15109901, 3.59555194, 21.98924882], - ] - - expected_controls = [ - [-20.54589655, 26.78617256, 15.76500633, np.nan], - [0.0, 0.0, 0.0, np.nan], - ] - - expected_iterations = [ - 212, - 224, - ] # while the number of iterations is reproductible, it differs between debug and raw runs - np.testing.assert_almost_equal(sol.states["q"], expected_q) - np.testing.assert_almost_equal(sol.states["qdot"], expected_qdot) - np.testing.assert_almost_equal(sol.controls["tau"], expected_controls) - assert sol.iterations in expected_iterations # second pass ocp_second_pass = ocp_module.prepare_ocp_second_pass( biorbd_model_path=model_path, n_threads=1, solution=sol @@ -64,79 +62,79 @@ def test_continuity_as_objective(phase_dynamics): sol_second_pass = ocp_second_pass.solve() # check q in integrality, qdot,controls, vector, cost, iterations - expected_q = [ - [0.0, -1.19252768, -1.42504253, -0.1376], - [0.0, 1.16885905, 1.88734408, 2.9976372], - ] - expected_qdot = [ - [0.0, -5.43603845, 2.40642094, 24.89415277], - [0.0, 5.15237646, 3.59628495, 21.99866436], - ] - expected_vector = [ - [ - 0, - 0, - 0, - 0, - -1.19253, - 1.16886, - -5.43604, - 5.15238, - -1.42504, - 1.88734, - 2.40642, - 3.59628, - -0.1376, - 2.99764, - 24.8942, - 21.9987, - -20.5528, - 0, - 26.7991, - 0, - 15.7774, - 0, - 0.553202, - ] - ] - - expected_constraints = [ - [ - -9.10383e-15, - 5.10703e-15, - 6.21725e-15, - 8.88178e-16, - 5.10703e-15, - -5.9952e-15, - 4.26326e-14, - 2.08722e-14, - 3.91354e-15, - 1.15463e-14, - 8.52651e-14, - 9.9476e-14, - 0, - -3.20577e-15, - -1.4988e-14, - 1.08612, - 0.35, - 0.37334, - 0.954385, - 0.458212, - 0.692135, - 0.492353, - 0.996479, - 1.63013, - 1.34023, - 1.15602, - 1.29985, - 1.97149, - 1.87178, - 1.94454, - 2.89311, - 2.74089, - 2.67422, - ] - ] + # expected_q = [ + # [0.0, -1.19252768, -1.42504253, -0.1376], + # [0.0, 1.16885905, 1.88734408, 2.9976372], + # ] + # expected_qdot = [ + # [0.0, -5.43603845, 2.40642094, 24.89415277], + # [0.0, 5.15237646, 3.59628495, 21.99866436], + # ] + # expected_vector = [ + # [ + # 0, + # 0, + # 0, + # 0, + # -1.19253, + # 1.16886, + # -5.43604, + # 5.15238, + # -1.42504, + # 1.88734, + # 2.40642, + # 3.59628, + # -0.1376, + # 2.99764, + # 24.8942, + # 21.9987, + # -20.5528, + # 0, + # 26.7991, + # 0, + # 15.7774, + # 0, + # 0.553202, + # ] + # ] + # + # expected_constraints = [ + # [ + # -9.10383e-15, + # 5.10703e-15, + # 6.21725e-15, + # 8.88178e-16, + # 5.10703e-15, + # -5.9952e-15, + # 4.26326e-14, + # 2.08722e-14, + # 3.91354e-15, + # 1.15463e-14, + # 8.52651e-14, + # 9.9476e-14, + # 0, + # -3.20577e-15, + # -1.4988e-14, + # 1.08612, + # 0.35, + # 0.37334, + # 0.954385, + # 0.458212, + # 0.692135, + # 0.492353, + # 0.996479, + # 1.63013, + # 1.34023, + # 1.15602, + # 1.29985, + # 1.97149, + # 1.87178, + # 1.94454, + # 2.89311, + # 2.74089, + # 2.67422, + # ] + # ] expected_cost = 311.551 @@ -153,14 +151,14 @@ def test_continuity_as_objective(phase_dynamics): }, ] - expected_iterations = [8] # run it TBD - np.testing.assert_almost_equal(sol_second_pass.states["q"], expected_q) - np.testing.assert_almost_equal(sol_second_pass.states["qdot"], expected_qdot) - np.testing.assert_almost_equal(sol_second_pass.vector.T, expected_vector, decimal=1) - np.testing.assert_almost_equal( - sol_second_pass.constraints.T, expected_constraints, decimal=1 - ) - np.testing.assert_almost_equal( - float(sol_second_pass.cost), expected_cost, decimal=2 - ) - assert sol_second_pass.iterations in expected_iterations + # expected_iterations = range(8, 12) + # np.testing.assert_almost_equal(sol_second_pass.states["q"], expected_q) + # np.testing.assert_almost_equal(sol_second_pass.states["qdot"], expected_qdot) + # np.testing.assert_almost_equal(sol_second_pass.vector.T, expected_vector, decimal=1) + # np.testing.assert_almost_equal( + # sol_second_pass.constraints.T, expected_constraints, decimal=1 + # ) + # np.testing.assert_almost_equal( + # float(sol_second_pass.cost), expected_cost, decimal=2 + # ) + # assert sol_second_pass.iterations in expected_iterations From 6ec857535c550f9f02629e1659dbeb294cc1ffbf Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 11:53:12 -0500 Subject: [PATCH 03/12] tests(restore continuity as objective) : removing minimizing time because it add noise to the test --- tests/shard1/test_continuity_as_objective.py | 228 +++++++++---------- 1 file changed, 106 insertions(+), 122 deletions(-) diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index 716e911ae..56bac7dd0 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -1,7 +1,7 @@ -from bioptim import PhaseDynamics import numpy as np import pytest +from bioptim import PhaseDynamics, SolutionMerge from tests.utils import TestUtils @@ -15,10 +15,7 @@ def test_continuity_as_objective(phase_dynamics): ) np.random.seed(42) - model_path = ( - TestUtils.bioptim_folder() - + "/examples/getting_started/models/pendulum_maze.bioMod" - ) + model_path = TestUtils.bioptim_folder() + "/examples/getting_started/models/pendulum_maze.bioMod" # first pass ocp = ocp_module.prepare_ocp_first_pass( @@ -26,139 +23,126 @@ def test_continuity_as_objective(phase_dynamics): n_threads=1, final_time=1, n_shooting=3, - state_continuity_weight=1000000, + state_continuity_weight=1000, phase_dynamics=phase_dynamics, expand_dynamics=True, + minimize_time=False, # we only want to test the continuity as objective here ) sol = ocp.solve() - # # check q in integrality, qdot, controls, iterations - # - # expected_q = [ - # [0.0, -1.1923517, -1.42486622, -0.1376], - # [0.0, 1.16898343, 1.88747861, 2.9976372], - # ] - # - # expected_qdot = [ - # [0.0, -5.43509473, 2.40522723, 24.88229668], - # [0.0, 5.15109901, 3.59555194, 21.98924882], - # ] - # - # expected_controls = [ - # [-20.54589655, 26.78617256, 15.76500633, np.nan], - # [0.0, 0.0, 0.0, np.nan], - # ] + expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] + + expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] + + expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] # - # expected_iterations = range(200, 250) # while the number of iterations is reproductible, it differs between debug and raw runs - # np.testing.assert_almost_equal(sol.states["q"], expected_q) - # np.testing.assert_almost_equal(sol.states["qdot"], expected_qdot) - # np.testing.assert_almost_equal(sol.controls["tau"], expected_controls) - # assert sol.iterations in expected_iterations + expected_iterations = range(300, 600) # 436 on my laptop @ipuch + np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["q"], expected_q) + np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["qdot"], expected_qdot) + np.testing.assert_almost_equal(sol.decision_controls(to_merge=SolutionMerge.NODES)["tau"], expected_controls) + assert sol.iterations in expected_iterations # second pass ocp_second_pass = ocp_module.prepare_ocp_second_pass( - biorbd_model_path=model_path, n_threads=1, solution=sol + biorbd_model_path=model_path, + n_threads=1, + n_shooting=3, + solution=sol, + minimize_time=False, ) sol_second_pass = ocp_second_pass.solve() # check q in integrality, qdot,controls, vector, cost, iterations - # expected_q = [ - # [0.0, -1.19252768, -1.42504253, -0.1376], - # [0.0, 1.16885905, 1.88734408, 2.9976372], - # ] - # expected_qdot = [ - # [0.0, -5.43603845, 2.40642094, 24.89415277], - # [0.0, 5.15237646, 3.59628495, 21.99866436], - # ] - # expected_vector = [ - # [ - # 0, - # 0, - # 0, - # 0, - # -1.19253, - # 1.16886, - # -5.43604, - # 5.15238, - # -1.42504, - # 1.88734, - # 2.40642, - # 3.59628, - # -0.1376, - # 2.99764, - # 24.8942, - # 21.9987, - # -20.5528, - # 0, - # 26.7991, - # 0, - # 15.7774, - # 0, - # 0.553202, - # ] - # ] - # - # expected_constraints = [ - # [ - # -9.10383e-15, - # 5.10703e-15, - # 6.21725e-15, - # 8.88178e-16, - # 5.10703e-15, - # -5.9952e-15, - # 4.26326e-14, - # 2.08722e-14, - # 3.91354e-15, - # 1.15463e-14, - # 8.52651e-14, - # 9.9476e-14, - # 0, - # -3.20577e-15, - # -1.4988e-14, - # 1.08612, - # 0.35, - # 0.37334, - # 0.954385, - # 0.458212, - # 0.692135, - # 0.492353, - # 0.996479, - # 1.63013, - # 1.34023, - # 1.15602, - # 1.29985, - # 1.97149, - # 1.87178, - # 1.94454, - # 2.89311, - # 2.74089, - # 2.67422, - # ] - # ] - - expected_cost = 311.551 + expected_q = [[0.0, -0.12359617, 0.21522375, -0.1376], [0.0, 0.06184961, -0.37118107, 2.9976372]] + expected_qdot = [[0.0, 0.14235975, -1.10526128, -4.21797828], [0.0, -0.55992744, 1.17407988, -1.06473819]] + expected_tau = [[-1.16548046, 1.10283517, -27.94121882], [0.0, 0.0, 0.0]] + + expected_vector = [ + 0.333333, + 0, + 0, + 0, + 0, + -0.123596, + 0.0618496, + 0.14236, + -0.559927, + 0.215224, + -0.371181, + -1.10526, + 1.17408, + -0.1376, + 2.99764, + -4.21798, + -1.06474, + -1.16548, + 0, + 1.10284, + 0, + -27.9412, + 0, + ] + expected_constraints = [ + 1.90126e-15, + 1.99146e-15, + -3.33067e-16, + -1.11022e-16, + 6.10623e-16, + 5.60663e-15, + 1.55431e-15, + -1.33227e-15, + -1.47937e-14, + -5.77316e-15, + -1.1573e-12, + -5.08926e-13, + 0, + -1.30451e-15, + -4.04121e-14, + 1.08612, + 1.05124, + 0.991253, + 0.954385, + 0.949236, + 0.9216, + 0.492353, + 0.554696, + 0.620095, + 1.34023, + 1.36917, + 1.38148, + 1.97149, + 2.0114, + 2.03747, + 2.89311, + 2.93228, + 2.95656, + ] + + expected_cost = 261.095 expected_detailed_cost = [ { - "cost_value": 22.02370080865957, - "cost_value_weighted": 256.23075256985715, "name": "Lagrange.MINIMIZE_CONTROL", - }, - { - "cost_value": 0.5532019998370968, - "cost_value_weighted": 55.320199983709685, - "name": "Mayer.MINIMIZE_TIME", - }, + "cost_value_weighted": 261.0954331500721, + "cost_value": -28.003864114406223, + } ] - # expected_iterations = range(8, 12) - # np.testing.assert_almost_equal(sol_second_pass.states["q"], expected_q) - # np.testing.assert_almost_equal(sol_second_pass.states["qdot"], expected_qdot) - # np.testing.assert_almost_equal(sol_second_pass.vector.T, expected_vector, decimal=1) - # np.testing.assert_almost_equal( - # sol_second_pass.constraints.T, expected_constraints, decimal=1 - # ) - # np.testing.assert_almost_equal( - # float(sol_second_pass.cost), expected_cost, decimal=2 - # ) - # assert sol_second_pass.iterations in expected_iterations + expected_iterations = range(5, 35) # 20 on my laptop @ipuch + np.testing.assert_almost_equal(sol_second_pass.decision_states(to_merge=SolutionMerge.NODES)["q"], expected_q) + np.testing.assert_almost_equal(sol_second_pass.decision_states(to_merge=SolutionMerge.NODES)["qdot"], expected_qdot) + np.testing.assert_almost_equal(sol_second_pass.decision_controls(to_merge=SolutionMerge.NODES)["tau"], expected_tau) + + np.testing.assert_almost_equal(sol_second_pass.vector, np.array([expected_vector]).T, decimal=4) + np.testing.assert_almost_equal(sol_second_pass.constraints, np.array([expected_constraints]).T, decimal=4) + np.testing.assert_almost_equal(float(sol_second_pass.cost), expected_cost, decimal=2) + + assert sol_second_pass.detailed_cost[0]["name"] == expected_detailed_cost[0]["name"] + np.testing.assert_almost_equal( + sol_second_pass.detailed_cost[0]["cost_value_weighted"], expected_detailed_cost[0]["cost_value_weighted"] + ) + np.testing.assert_almost_equal( + sol_second_pass.detailed_cost[0]["cost_value"], expected_detailed_cost[0]["cost_value"] + ) + + assert sol_second_pass.iterations in expected_iterations From 9d55cf84fe5acaad0bb8477a45f7ea090ea51f3d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 11:55:24 -0500 Subject: [PATCH 04/12] example(continuity as objective) : restoring doc, minimize_time as bool for tests, dividing weights of minimize time by the n_shooting, second pass converge @703 iterations --- .../example_continuity_as_objective.py | 74 ++++++++++++------- 1 file changed, 47 insertions(+), 27 deletions(-) diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index ade940231..e58be7920 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -15,8 +15,9 @@ import platform -from casadi import sqrt import numpy as np +from casadi import sqrt + from bioptim import ( BiorbdModel, OptimalControlProgram, @@ -51,15 +52,16 @@ def out_of_sphere(controller: PenaltyController, y, z): def prepare_ocp_first_pass( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - state_continuity_weight: float, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, + biorbd_model_path: str, + final_time: float, + n_shooting: int, + state_continuity_weight: float, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp @@ -89,6 +91,8 @@ def prepare_ocp_first_pass( 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) + minimize_time: bool + If the time should be minimized Returns ------- @@ -100,7 +104,8 @@ def prepare_ocp_first_pass( # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, weight=1, key="tau") - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=100) + if minimize_time: + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=100 / n_shooting) # Dynamics dynamics = Dynamics( @@ -122,7 +127,7 @@ def prepare_ocp_first_pass( n_qdot = bio_model.nb_qdot x_init = InitialGuessList() x_init["q"] = [0] * n_q - x_init["qdot"] = [0] * n_q + x_init["qdot"] = [0] * n_qdot x_init.add_noise(bounds=x_bounds, magnitude=0.001, n_shooting=n_shooting + 1) # Define control path constraint @@ -132,9 +137,8 @@ def prepare_ocp_first_pass( u_bounds["tau"] = [tau_min] * n_tau, [tau_max] * n_tau u_bounds["tau"][1, :] = 0 # Prevent the model from actively rotate - controls = solution.decision_controls(to_merge=SolutionMerge.NODES) u_init = InitialGuessList() - u_init.add("tau", controls["tau"], interpolation=InterpolationType.EACH_FRAME) + u_init["tau"] = [tau_init] * n_tau u_init.add_noise(bounds=u_bounds, magnitude=0.01, n_shooting=n_shooting) constraints = ConstraintList() @@ -212,11 +216,13 @@ def prepare_ocp_first_pass( def prepare_ocp_second_pass( - biorbd_model_path: str, - solution: Solution, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, + biorbd_model_path: str, + n_shooting: int, + solution: Solution, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp @@ -225,12 +231,18 @@ def prepare_ocp_second_pass( ---------- biorbd_model_path: str The path to the biorbd model + n_shooting: int + The number of shooting points to define int the direct multiple shooting program + solution: Solution + The first pass solution ode_solver: OdeSolverBase = OdeSolver.RK4() Which type of OdeSolver to use use_sx: bool If the SX variable should be used instead of MX (can be extensive on RAM) n_threads: int The number of threads to use in the paralleling (1 = no parallel computing) + minimize_time: bool + If the time should be minimized Returns ------- @@ -242,7 +254,8 @@ def prepare_ocp_second_pass( # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, weight=1, key="tau") - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=100) + if minimize_time: + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=100 / n_shooting) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) @@ -255,10 +268,15 @@ def prepare_ocp_second_pass( x_bounds["qdot"][:, 0] = 0 # Initial guess - states = solution.decision_states(to_merge=SolutionMerge.NODES) x_init = InitialGuessList() - x_init.add("q", states["q"], interpolation=InterpolationType.EACH_FRAME) - x_init.add("qdot", states["qdot"], interpolation=InterpolationType.EACH_FRAME) + x_init.add( + "q", solution.decision_states(to_merge=SolutionMerge.NODES)["q"], interpolation=InterpolationType.EACH_FRAME + ) + x_init.add( + "qdot", + solution.decision_states(to_merge=SolutionMerge.NODES)["qdot"], + interpolation=InterpolationType.EACH_FRAME, + ) # Define control path constraint n_tau = bio_model.nb_tau @@ -270,7 +288,7 @@ def prepare_ocp_second_pass( u_init = InitialGuessList() u_init.add( "tau", - solution.controls["tau"][:, :-1], + solution.decision_controls(to_merge=SolutionMerge.NODES)["tau"], interpolation=InterpolationType.EACH_FRAME, ) @@ -331,11 +349,13 @@ def prepare_ocp_second_pass( node=Node.ALL_SHOOTING, ) + final_time = float(solution.decision_time(to_merge=SolutionMerge.NODES)[-1, 0]) + return OptimalControlProgram( bio_model, dynamics, - solution.ns, - solution.phase_time[-1], + n_shooting, + final_time, x_init=x_init, u_init=u_init, x_bounds=x_bounds, @@ -391,7 +411,7 @@ def main(): solver_second.set_maximum_iterations(10000) ocp_second = prepare_ocp_second_pass( - biorbd_model_path="models/pendulum_maze.bioMod", solution=sol_first, n_threads=3 + biorbd_model_path="models/pendulum_maze.bioMod", n_shooting=500, solution=sol_first, n_threads=3 ) # Custom plots From 5f0b3e58cbf153bbd736db595761ef18411ea982 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 11:59:57 -0500 Subject: [PATCH 05/12] lint(manually) --- tests/shard1/test_continuity_as_objective.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index 56bac7dd0..173a32d11 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -31,12 +31,10 @@ def test_continuity_as_objective(phase_dynamics): sol = ocp.solve() expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] - expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] - expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] - # expected_iterations = range(300, 600) # 436 on my laptop @ipuch + np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["q"], expected_q) np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["qdot"], expected_qdot) np.testing.assert_almost_equal(sol.decision_controls(to_merge=SolutionMerge.NODES)["tau"], expected_controls) @@ -51,7 +49,6 @@ def test_continuity_as_objective(phase_dynamics): minimize_time=False, ) sol_second_pass = ocp_second_pass.solve() - # check q in integrality, qdot,controls, vector, cost, iterations expected_q = [[0.0, -0.12359617, 0.21522375, -0.1376], [0.0, 0.06184961, -0.37118107, 2.9976372]] expected_qdot = [[0.0, 0.14235975, -1.10526128, -4.21797828], [0.0, -0.55992744, 1.17407988, -1.06473819]] @@ -119,7 +116,6 @@ def test_continuity_as_objective(phase_dynamics): ] expected_cost = 261.095 - expected_detailed_cost = [ { "name": "Lagrange.MINIMIZE_CONTROL", @@ -127,8 +123,8 @@ def test_continuity_as_objective(phase_dynamics): "cost_value": -28.003864114406223, } ] - expected_iterations = range(5, 35) # 20 on my laptop @ipuch + np.testing.assert_almost_equal(sol_second_pass.decision_states(to_merge=SolutionMerge.NODES)["q"], expected_q) np.testing.assert_almost_equal(sol_second_pass.decision_states(to_merge=SolutionMerge.NODES)["qdot"], expected_qdot) np.testing.assert_almost_equal(sol_second_pass.decision_controls(to_merge=SolutionMerge.NODES)["tau"], expected_tau) From 64908662fecd31e96970e719bf4a8aece9ea9569 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 12:01:20 -0500 Subject: [PATCH 06/12] lint(black) --- .../example_continuity_as_objective.py | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index e58be7920..9654fe454 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -52,16 +52,16 @@ def out_of_sphere(controller: PenaltyController, y, z): def prepare_ocp_first_pass( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - state_continuity_weight: float, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, - minimize_time: bool = True, + biorbd_model_path: str, + final_time: float, + n_shooting: int, + state_continuity_weight: float, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp @@ -216,13 +216,13 @@ def prepare_ocp_first_pass( def prepare_ocp_second_pass( - biorbd_model_path: str, - n_shooting: int, - solution: Solution, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - minimize_time: bool = True, + biorbd_model_path: str, + n_shooting: int, + solution: Solution, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp From 51f79dbe0a3aee5c4fc1409b6d0a0871b7e40e4b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 12:12:15 -0500 Subject: [PATCH 07/12] one line for contraints --- .../example_continuity_as_objective.py | 157 ++++-------------- 1 file changed, 31 insertions(+), 126 deletions(-) diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index 9654fe454..575b2ed89 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -52,16 +52,16 @@ def out_of_sphere(controller: PenaltyController, y, z): def prepare_ocp_first_pass( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - state_continuity_weight: float, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, - minimize_time: bool = True, + biorbd_model_path: str, + final_time: float, + n_shooting: int, + state_continuity_weight: float, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp @@ -142,61 +142,14 @@ def prepare_ocp_first_pass( u_init.add_noise(bounds=u_bounds, magnitude=0.01, n_shooting=n_shooting) constraints = ConstraintList() - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - node=Node.END, - first_marker="marker_2", - second_marker="target_2", - ) - constraints.add( - out_of_sphere, - y=-0.45, - z=0, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=0.05, - z=0, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) + constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="marker_2", second_marker="target_2") + constraints.add(out_of_sphere, y=-0.45, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=0.05, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) # for another good example, comment out this line below here and in second pass (see HERE) - constraints.add( - out_of_sphere, - y=0.55, - z=-0.85, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=0.75, - z=0.2, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=1.4, - z=0.5, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=2, - z=1.2, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) + constraints.add(out_of_sphere, y=0.55, z=-0.85, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=0.75, z=0.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=1.4, z=0.5, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=2, z=1.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) return OptimalControlProgram( bio_model, @@ -216,13 +169,13 @@ def prepare_ocp_first_pass( def prepare_ocp_second_pass( - biorbd_model_path: str, - n_shooting: int, - solution: Solution, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - minimize_time: bool = True, + biorbd_model_path: str, + n_shooting: int, + solution: Solution, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp @@ -293,61 +246,13 @@ def prepare_ocp_second_pass( ) constraints = ConstraintList() - constraints.add( - ConstraintFcn.SUPERIMPOSE_MARKERS, - node=Node.END, - first_marker="marker_2", - second_marker="target_2", - ) - constraints.add( - out_of_sphere, - y=-0.45, - z=0, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=0.05, - z=0, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - # HERE (referenced in first pass) - constraints.add( - out_of_sphere, - y=0.55, - z=-0.85, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=0.75, - z=0.2, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=1.4, - z=0.5, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) - constraints.add( - out_of_sphere, - y=2, - z=1.2, - min_bound=0.35, - max_bound=np.inf, - node=Node.ALL_SHOOTING, - ) + constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="marker_2", second_marker="target_2") + constraints.add(out_of_sphere, y=-0.45, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=0.05, z=0, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=0.55, z=-0.85, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=0.75, z=0.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=1.4, z=0.5, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) + constraints.add(out_of_sphere, y=2, z=1.2, min_bound=0.35, max_bound=np.inf, node=Node.ALL_SHOOTING) final_time = float(solution.decision_time(to_merge=SolutionMerge.NODES)[-1, 0]) From 45fa46969230c45a7dc8b79e25a1f814021ccb0e Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 13:31:49 -0500 Subject: [PATCH 08/12] black again --- .../example_continuity_as_objective.py | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index 575b2ed89..d3e0289b5 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -52,16 +52,16 @@ def out_of_sphere(controller: PenaltyController, y, z): def prepare_ocp_first_pass( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - state_continuity_weight: float, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, - minimize_time: bool = True, + biorbd_model_path: str, + final_time: float, + n_shooting: int, + state_continuity_weight: float, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp @@ -169,13 +169,13 @@ def prepare_ocp_first_pass( def prepare_ocp_second_pass( - biorbd_model_path: str, - n_shooting: int, - solution: Solution, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - minimize_time: bool = True, + biorbd_model_path: str, + n_shooting: int, + solution: Solution, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + minimize_time: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp From aa1444cd3a2dc415e7d7c892c38a7a73530c2633 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 13:39:50 -0500 Subject: [PATCH 09/12] fix(tests) : continuity as objective, more precision on tested values --- tests/shard1/test_continuity_as_objective.py | 122 +++++++++---------- 1 file changed, 61 insertions(+), 61 deletions(-) diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index 173a32d11..a0c098a58 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -33,7 +33,7 @@ def test_continuity_as_objective(phase_dynamics): expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] - expected_iterations = range(300, 600) # 436 on my laptop @ipuch + expected_iterations = range(300, 700) # 436 on my laptop @ipuch, 639 on Windows Github CI np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["q"], expected_q) np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["qdot"], expected_qdot) @@ -55,67 +55,67 @@ def test_continuity_as_objective(phase_dynamics): expected_tau = [[-1.16548046, 1.10283517, -27.94121882], [0.0, 0.0, 0.0]] expected_vector = [ - 0.333333, - 0, - 0, - 0, - 0, - -0.123596, - 0.0618496, - 0.14236, - -0.559927, - 0.215224, - -0.371181, - -1.10526, - 1.17408, - -0.1376, - 2.99764, - -4.21798, - -1.06474, - -1.16548, - 0, - 1.10284, - 0, - -27.9412, - 0, + [0.333333333333333], + [0.0], + [0.0], + [0.0], + [0.0], + [-0.123596172283089], + [0.06184960919155], + [0.142359752273267], + [-0.559927440453811], + [0.215223745187429], + [-0.371181072742915], + [-1.105261281320264], + [1.174079883672672], + [-0.137599999999438], + [2.99763720171466], + [-4.217978284100386], + [-1.064738187592947], + [-1.16548046161843], + [0.0], + [1.102835170494119], + [0.0], + [-27.94121882328191], + [0.0], ] expected_constraints = [ - 1.90126e-15, - 1.99146e-15, - -3.33067e-16, - -1.11022e-16, - 6.10623e-16, - 5.60663e-15, - 1.55431e-15, - -1.33227e-15, - -1.47937e-14, - -5.77316e-15, - -1.1573e-12, - -5.08926e-13, - 0, - -1.30451e-15, - -4.04121e-14, - 1.08612, - 1.05124, - 0.991253, - 0.954385, - 0.949236, - 0.9216, - 0.492353, - 0.554696, - 0.620095, - 1.34023, - 1.36917, - 1.38148, - 1.97149, - 2.0114, - 2.03747, - 2.89311, - 2.93228, - 2.95656, + [0.000000000000002], + [0.000000000000002], + [-0.0], + [-0.0], + [0.000000000000001], + [0.000000000000006], + [0.000000000000002], + [-0.000000000000001], + [-0.000000000000015], + [-0.000000000000006], + [-0.000000000001157], + [-0.000000000000509], + [0.0], + [-0.000000000000001], + [-0.00000000000004], + [1.086117433798022], + [1.051237343203642], + [0.991252668915217], + [0.954385184294056], + [0.949235691291338], + [0.921600419952798], + [0.492352597230887], + [0.554696150939241], + [0.620095068306614], + [1.340227995529119], + [1.369169603766323], + [1.381480510093951], + [1.97149463098432], + [2.011400546027982], + [2.037470886662517], + [2.893114425666568], + [2.932275980994855], + [2.956560343780495], ] - expected_cost = 261.095 + expected_cost = 261.0954331500721 expected_detailed_cost = [ { "name": "Lagrange.MINIMIZE_CONTROL", @@ -129,9 +129,9 @@ def test_continuity_as_objective(phase_dynamics): np.testing.assert_almost_equal(sol_second_pass.decision_states(to_merge=SolutionMerge.NODES)["qdot"], expected_qdot) np.testing.assert_almost_equal(sol_second_pass.decision_controls(to_merge=SolutionMerge.NODES)["tau"], expected_tau) - np.testing.assert_almost_equal(sol_second_pass.vector, np.array([expected_vector]).T, decimal=4) - np.testing.assert_almost_equal(sol_second_pass.constraints, np.array([expected_constraints]).T, decimal=4) - np.testing.assert_almost_equal(float(sol_second_pass.cost), expected_cost, decimal=2) + np.testing.assert_almost_equal(sol_second_pass.vector, expected_vector) + np.testing.assert_almost_equal(sol_second_pass.constraints, expected_constraints) + np.testing.assert_almost_equal(float(sol_second_pass.cost), expected_cost) assert sol_second_pass.detailed_cost[0]["name"] == expected_detailed_cost[0]["name"] np.testing.assert_almost_equal( From 5936b8ee1c64db6beb38d585c4cc8bcbed2eefff Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 15:09:13 -0500 Subject: [PATCH 10/12] go for multiplateform testing --- tests/shard1/test_continuity_as_objective.py | 26 ++++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index a0c098a58..fe1cd1fb1 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -1,3 +1,5 @@ +import os + import numpy as np import pytest @@ -30,15 +32,29 @@ def test_continuity_as_objective(phase_dynamics): ) sol = ocp.solve() - expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] - expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] - expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] - expected_iterations = range(300, 700) # 436 on my laptop @ipuch, 639 on Windows Github CI + if os.sys.platform == "windows": + expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] + expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] + expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] + expected_iterations = range(300, 700) # 436 on my laptop @ipuch, 639 on Windows Github CI + + if os.sys.platform == "linux" or os.sys.platform == "darwin": + np.printoptions(precision=15, suppress=True) + + print(sol.decision_states(to_merge=SolutionMerge.NODES)["q"]) + print(sol.decision_states(to_merge=SolutionMerge.NODES)["qdot"]) + print(sol.decision_controls(to_merge=SolutionMerge.NODES)["tau"]) + print(sol.iterations) + expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] + expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] + expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] + expected_iterations = range(300, 700) + + assert sol.iterations in expected_iterations np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["q"], expected_q) np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["qdot"], expected_qdot) np.testing.assert_almost_equal(sol.decision_controls(to_merge=SolutionMerge.NODES)["tau"], expected_controls) - assert sol.iterations in expected_iterations # second pass ocp_second_pass = ocp_module.prepare_ocp_second_pass( From 51ea68bde1faca991bd8c7543192aba9f95d33e1 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 15:30:06 -0500 Subject: [PATCH 11/12] tests:(Try again) --- tests/shard1/test_continuity_as_objective.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index fe1cd1fb1..727ee4b13 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -1,4 +1,4 @@ -import os +import platform import numpy as np import pytest @@ -32,13 +32,15 @@ def test_continuity_as_objective(phase_dynamics): ) sol = ocp.solve() - if os.sys.platform == "windows": + # 436 on my laptop @ipuch, 639 on Windows Github CI, 192 on Linux Github CI + expected_iterations = range(100, 700) + + if platform.system() == "Windows": expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] - expected_iterations = range(300, 700) # 436 on my laptop @ipuch, 639 on Windows Github CI - if os.sys.platform == "linux" or os.sys.platform == "darwin": + if platform.system() == "Linux" or platform.system() == "Darwin": np.printoptions(precision=15, suppress=True) print(sol.decision_states(to_merge=SolutionMerge.NODES)["q"]) @@ -46,10 +48,9 @@ def test_continuity_as_objective(phase_dynamics): print(sol.decision_controls(to_merge=SolutionMerge.NODES)["tau"]) print(sol.iterations) - expected_q = [[0.0, -0.1820716, 0.0502083, -0.1376], [0.0, 0.2059882, -0.3885045, 2.9976372]] - expected_qdot = [[0.0, 0.13105439, -3.43794783, -23.6570729], [0.0, -0.66178869, 3.07970721, -19.12526049]] - expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] - expected_iterations = range(300, 700) + expected_q = [[0.0, -0.17103307, 0.07459213, -0.1376], [0.0, 0.20294463, -0.38390195, 2.9976372]] + expected_qdot = [[0.0, 0.14587462, -3.35487788, 7.53981222], [0.0, -0.66021714, 3.02208876, 9.54451337]] + expected_controls = [[-1.47014529, -0.22059134, -18.23601047], [0.0, 0.0, 0.0]] assert sol.iterations in expected_iterations np.testing.assert_almost_equal(sol.decision_states(to_merge=SolutionMerge.NODES)["q"], expected_q) From 4856e37a9ae5eb9d683e05046b2131cac47de864 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 4 Mar 2024 15:40:45 -0500 Subject: [PATCH 12/12] tests(works on every plateform for continuity as objective) --- tests/shard1/test_continuity_as_objective.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/tests/shard1/test_continuity_as_objective.py b/tests/shard1/test_continuity_as_objective.py index 727ee4b13..bbd329a59 100644 --- a/tests/shard1/test_continuity_as_objective.py +++ b/tests/shard1/test_continuity_as_objective.py @@ -41,13 +41,7 @@ def test_continuity_as_objective(phase_dynamics): expected_controls = [[-1.49607534, -0.24541618, -19.12881238], [0.0, 0.0, 0.0]] if platform.system() == "Linux" or platform.system() == "Darwin": - np.printoptions(precision=15, suppress=True) - - print(sol.decision_states(to_merge=SolutionMerge.NODES)["q"]) - print(sol.decision_states(to_merge=SolutionMerge.NODES)["qdot"]) - print(sol.decision_controls(to_merge=SolutionMerge.NODES)["tau"]) - print(sol.iterations) - + # it lands on another local minima expected_q = [[0.0, -0.17103307, 0.07459213, -0.1376], [0.0, 0.20294463, -0.38390195, 2.9976372]] expected_qdot = [[0.0, 0.14587462, -3.35487788, 7.53981222], [0.0, -0.66021714, 3.02208876, 9.54451337]] expected_controls = [[-1.47014529, -0.22059134, -18.23601047], [0.0, 0.0, 0.0]]