def test_align_segment_on_rt(ode_solver): ocp = align_segment_on_rt.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod", final_time=0.5, number_shooting_points=8, ode_solver=ode_solver, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 12320.059717265229) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (91, 1)) np.testing.assert_almost_equal(g, np.zeros((91, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0.305837645, 6.07684988e-18, -1.57, -1.57])) np.testing.assert_almost_equal(q[:, -1], np.array([0.305837645, 2.34331392e-17, 1.57, 1.57])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([1.09038782e-23, 9.81, 66.9866667, 66.9866667])) np.testing.assert_almost_equal(tau[:, -1], np.array([-1.61910771e-23, 9.81, -66.9866667, -66.9866667])) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_align_segment_on_rt(ode_solver): ocp = align_segment_on_rt.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod", final_time=0.5, number_shooting_points=8, ode_solver=ode_solver, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 197120.95524154368) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (91, 1)) np.testing.assert_almost_equal(g, np.zeros((91, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0.30543155, 0, -1.57, -1.57])) np.testing.assert_almost_equal(q[:, -1], np.array([0.30543155, 0, 1.57, 1.57])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([0, 9.81, 66.98666900582079, 66.98666424580644])) np.testing.assert_almost_equal(tau[:, -1], np.array([-0, 9.81, -66.98666900582079, -66.98666424580644])) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_custom_constraint_align_markers(ode_solver): ocp = custom_constraint.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod", ode_solver=ode_solver) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 19767.533125695223) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (186, 1)) np.testing.assert_almost_equal(g, np.zeros((186, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array((1.4516128810214546, 9.81, 2.2790322540381487))) np.testing.assert_almost_equal( tau[:, -1], np.array((-1.4516128810214546, 9.81, -2.2790322540381487)))
def save_and_load(sol, ocp, test_solve_of_loaded=False): file_path = "test.bo" ocp.save(sol, file_path) ocp_load, sol_load = OptimalControlProgram.load(file_path) TestUtils.deep_assert(sol, sol_load) TestUtils.deep_assert(sol_load, sol) if test_solve_of_loaded: sol_from_load = ocp_load.solve() TestUtils.deep_assert(sol, sol_from_load) TestUtils.deep_assert(sol_from_load, sol) TestUtils.deep_assert(ocp_load, ocp) TestUtils.deep_assert(ocp, ocp_load) os.remove(file_path) file_path_bob = "test.bob" ocp.save_get_data(sol, file_path_bob, interpolate_nb_frames=-1, concatenate=True) data = Data.get_data(ocp, sol, file_path_bob, interpolate_nb_frames=-1, concatenate=True) with open(file_path_bob, "rb") as file: data_load = pickle.load(file)["data"] TestUtils.deep_assert(data, data_load) TestUtils.deep_assert(data_load, data) os.remove(file_path_bob)
def test_muscle_driven_ocp(): # Load static_arm PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "static_arm", str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/static_arm.py", ) static_arm = importlib.util.module_from_spec(spec) spec.loader.exec_module(static_arm) ocp = static_arm.prepare_ocp(str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod", final_time=2, number_shooting_points=10) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 0.14350464848810182) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1)), decimal=6) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau, mus = states["q"], states["q_dot"], controls[ "tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0.07, 1.4])) np.testing.assert_almost_equal(q[:, -1], np.array([-0.9451058, 3.0704789])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.4115254, -0.5586797])) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([0.0014793, 0.0052082])) np.testing.assert_almost_equal(tau[:, -1], np.array([-0.0002795, 0.0006926])) np.testing.assert_almost_equal( mus[:, 0], np.array([ 2.2869218e-06, 1.6503522e-01, 1.0002514e-01, 4.0190181e-06, 4.1294041e-06, 1.0396051e-01 ]), ) np.testing.assert_almost_equal( mus[:, -1], np.array([ 4.2599283e-03, 3.2188697e-05, 3.1307377e-05, 2.0121186e-03, 2.0048373e-03, 1.8235679e-03 ]), ) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_align_and_minimize_marker_velocity(): # Load align_and_minimize_marker_velocity PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_and_minimize_marker_velocity", str(PROJECT_FOLDER) + "/examples/align/align_and_minimize_marker_velocity.py", ) align_and_minimize_marker_velocity = importlib.util.module_from_spec(spec) spec.loader.exec_module(align_and_minimize_marker_velocity) ocp = align_and_minimize_marker_velocity.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod", number_shooting_points=5, final_time=1, marker_velocity_or_displacement="velo", marker_in_first_coordinates_system=True, control_type=ControlType.CONSTANT, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], -80.20048585400944) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal( q[:, 0], np.array([7.18708669e-01, -4.45703930e-01, -3.14159262e00, 0])) np.testing.assert_almost_equal( q[:, -1], np.array([1.08646846e00, -3.86731175e-01, 3.14159262e00, 0])) # initial and final velocities np.testing.assert_almost_equal( qdot[:, 0], np.array([3.78330878e-01, 3.70214281, 10, 0])) np.testing.assert_almost_equal( qdot[:, -1], np.array([3.77168521e-01, -3.40782793, 10, 0])) # # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array([-4.52216174e-02, 9.25170010e-01, 0, 0])) np.testing.assert_almost_equal(tau[:, -1], np.array([4.4260355e-02, 1.4004583, 0, 0])) # # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp)
def test_align_and_minimize_marker_velocity_linear_controls(): # Load align_and_minimize_marker_velocity PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_and_minimize_marker_velocity", str(PROJECT_FOLDER) + "/examples/align/align_and_minimize_marker_velocity.py", ) align_and_minimize_marker_velocity = importlib.util.module_from_spec(spec) spec.loader.exec_module(align_and_minimize_marker_velocity) ocp = align_and_minimize_marker_velocity.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod", number_shooting_points=5, final_time=1, marker_velocity_or_displacement="velo", marker_in_first_coordinates_system=True, control_type=ControlType.LINEAR_CONTINUOUS, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], -80.28869898410233) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal( q[:, 0], np.array([0.79919667, -0.11729546, -3.1415926, 0])) np.testing.assert_almost_equal( q[:, -1], np.array([0.80144593, -0.10039572, 3.1415926, 0])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([1.8420249e-03, 1.0703158, 10, 0])) np.testing.assert_almost_equal( qdot[:, -1], np.array([2.9192347e-03, -0.96476530, 10, 0])) # # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array([5.6477202e-03, 3.9487332, -8.4955414, 0])) np.testing.assert_almost_equal( tau[:, -1], np.array([-9.9579303e-03, 4.0992343, 8.4955414, 0])) # # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp, decimal_value=6)
def test_state_transitions(): # Load state_transitions PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "state_transitions", str(PROJECT_FOLDER) + "/examples/getting_started/custom_phase_transitions.py") state_transitions = importlib.util.module_from_spec(spec) spec.loader.exec_module(state_transitions) ocp = state_transitions.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod") sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 110875.0772043361) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (515, 1)) np.testing.assert_almost_equal(g, np.zeros((515, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"], concatenate=False) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[0][:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[-1][:, -1], np.array((1, 0, 0))) # initial and final velocities np.testing.assert_almost_equal(qdot[0][:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[-1][:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal( tau[0][:, 0], np.array((0.9598672, 9.7085598, -0.0623733))) np.testing.assert_almost_equal(tau[-1][:, -1], np.array((0, 1.2717052e01, 1.1487805e00))) # cyclic continuity (between phase 3 and phase 0) np.testing.assert_almost_equal(q[-1][:, -1], q[0][:, 0]) # Continuity between phase 0 and phase 1 np.testing.assert_almost_equal(q[0][:, -1], q[1][:, 0]) # save and load with pytest.raises(PicklingError, match="import of module 'state_transitions' failed"): TestUtils.save_and_load(sol, ocp, True) # simulate with pytest.raises(AssertionError, match="Arrays are not almost equal to 7 decimals"): TestUtils.simulate(sol, ocp)
def test_muscle_with_contact_driven_ocp(): ocp = static_arm_with_contact.prepare_ocp( str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26_with_contact.bioMod", final_time=2, number_shooting_points=10, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 0.12145850795787627) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (60, 1)) np.testing.assert_almost_equal(g, np.zeros((60, 1)), decimal=6) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau, mus = states["q"], states["q_dot"], controls[ "tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0, 0.07, 1.4])) np.testing.assert_almost_equal( q[:, -1], np.array([0.00806796, -0.95744612, 3.09501746])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0.0, 0.0])) np.testing.assert_almost_equal( qdot[:, -1], np.array([0.00060479, 0.34617159, -0.42910107])) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array([-4.67191123e-05, 4.58080354e-03, 4.23701953e-03])) np.testing.assert_almost_equal( tau[:, -1], np.array([-1.19345958e-05, -1.34810157e-03, 3.12378179e-03])) np.testing.assert_almost_equal( mus[:, 0], np.array([ 6.86280305e-06, 1.68961894e-01, 8.70635867e-02, 2.47160155e-05, 2.53946780e-05, 8.45438966e-02 ]), ) np.testing.assert_almost_equal( mus[:, -1], np.array([ 1.96721725e-02, 3.42398501e-05, 3.29454916e-05, 8.61757459e-03, 8.57946846e-03, 7.07152302e-03 ]), ) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_muscle_activations_and_states_tracking(): # Define the problem model_path = str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 2 nb_shooting = 9 # Generate random data to fit np.random.seed(42) t, markers_ref, x_ref, muscle_activations_ref = muscle_activations_tracker.generate_data( biorbd_model, final_time, nb_shooting ) biorbd_model = biorbd.Model(model_path) # To allow for non free variable, the model must be reloaded ocp = muscle_activations_tracker.prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, muscle_activations_ref, x_ref[: biorbd_model.nbQ(), :].T, kin_data_to_track="q", ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 1.4506639252752042e-06) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (36, 1)) np.testing.assert_almost_equal(g, np.zeros((36, 1)), decimal=6) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau, mus = states["q"], states["q_dot"], controls["tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([-1.13043502e-05, -1.35629661e-05])) np.testing.assert_almost_equal(q[:, -1], np.array([-0.49387966, -1.44924784])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([-8.66527631e-05, -1.31486656e-04])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.8780829, -2.6474387])) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([-1.55359644e-06, 1.26569700e-05])) np.testing.assert_almost_equal(tau[:, -1], np.array([-7.41845169e-06, -7.67568954e-07])) np.testing.assert_almost_equal( mus[:, 0], np.array([0.37439688, 0.95073361, 0.73203047, 0.59855246, 0.15592687, 0.15595739]), ) np.testing.assert_almost_equal( mus[:, -1], np.array([0.54685367, 0.18482085, 0.96945157, 0.77512036, 0.93947405, 0.89483397]), )
def test_multiphase_time_constraint(): # Load time_constraint PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "multiphase_time_constraint", str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/multiphase_time_constraint.py") multiphase_time_constraint = importlib.util.module_from_spec(spec) spec.loader.exec_module(multiphase_time_constraint) ocp = multiphase_time_constraint.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/cube.bioMod", final_time=(2, 5, 4), time_min=[1, 3, 0.1], time_max=[2, 4, 0.8], number_shooting_points=(20, 30, 20), ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 55582.04125059745) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (444, 1)) np.testing.assert_almost_equal(g, np.zeros((444, 1))) # Check some of the results states, controls, param = Data.get_data(ocp, sol["x"], get_parameters=True) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] tf = param["time"][0, 0] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((5.71428583, 9.81, 0))) np.testing.assert_almost_equal(tau[:, -1], np.array((-8.92857121, 9.81, -14.01785679))) # optimized time np.testing.assert_almost_equal(tf, 1.0) # save and load TestUtils.save_and_load(sol, ocp, True)
def test_multiphase_align_markers(ode_solver): ocp = multiphase_align_markers.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod", ode_solver=ode_solver) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 17672.950313589874) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (444, 1)) np.testing.assert_almost_equal(g, np.zeros((444, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"], concatenate=False) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[0][:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[0][:, -1], np.array((2, 0, 0))) np.testing.assert_almost_equal(q[1][:, 0], np.array((2, 0, 0))) np.testing.assert_almost_equal(q[1][:, -1], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[2][:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[2][:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[0][:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[0][:, -1], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[1][:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[1][:, -1], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[2][:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[2][:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[0][:, 0], np.array( (1.42857142, 9.81, 0))) np.testing.assert_almost_equal(tau[0][:, -1], np.array((-1.42857144, 9.81, 0))) np.testing.assert_almost_equal(tau[1][:, 0], np.array((-0.2322581, 9.81, 0.0))) np.testing.assert_almost_equal(tau[1][:, -1], np.array((0.2322581, 9.81, -0.0))) np.testing.assert_almost_equal(tau[2][:, 0], np.array((0.35714285, 9.81, 0.56071428))) np.testing.assert_almost_equal(tau[2][:, -1], np.array((-0.35714285, 9.81, -0.56071428))) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_cyclic_objective(): # Load initial_guess PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "initial_guess", str(PROJECT_FOLDER) + "/examples/getting_started/example_cyclic_movement.py") cyclic_movement = importlib.util.module_from_spec(spec) spec.loader.exec_module(cyclic_movement) np.random.seed(42) ocp = cyclic_movement.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod", final_time=1, number_shooting_points=10, loop_from_constraint=False, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 56851.881815451816) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (67, 1)) np.testing.assert_almost_equal(g, np.zeros((67, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal( q[:, 0], np.array([1.60205103, -0.01069317, 0.62477988])) np.testing.assert_almost_equal(q[:, -1], np.array([1, 0, 1.57])) # initial and final velocities np.testing.assert_almost_equal( qdot[:, 0], np.array((0.12902365, 0.09340155, -0.20256713))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array([9.89210954, 9.39362112, -15.53061197])) np.testing.assert_almost_equal( tau[:, -1], np.array([17.16370432, 9.78643138, -26.94701577])) # save and load TestUtils.save_and_load(sol, ocp, True) # simulate TestUtils.simulate(sol, ocp)
def test_time_constraint(ode_solver): # Load time_constraint PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "time_constraint", str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/time_constraint.py", ) time_constraint = importlib.util.module_from_spec(spec) spec.loader.exec_module(time_constraint) ocp = time_constraint.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/pendulum.bioMod", final_time=2, number_shooting_points=10, time_min=0.6, time_max=1, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 1451.2202233368012) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1))) # Check some of the results states, controls, param = Data.get_data(ocp, sol["x"], get_parameters=True) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] tf = param["time"][0, 0] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((22.49775, 0))) np.testing.assert_almost_equal(tau[:, -1], np.array((-33.9047809, 0))) # optimized time np.testing.assert_almost_equal(tf, 1.0) # save and load TestUtils.save_and_load(sol, ocp, True)
def test_muscle_driven_ocp(): ocp = static_arm.prepare_ocp(str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod", final_time=2, number_shooting_points=10) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 0.12145862454010191) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1)), decimal=6) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau, mus = states["q"], states["q_dot"], controls[ "tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0.07, 1.4])) np.testing.assert_almost_equal(q[:, -1], np.array([-0.95746379, 3.09503322])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.34608528, -0.42902204])) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([0.00458156, 0.00423722])) np.testing.assert_almost_equal(tau[:, -1], np.array([-0.00134814, 0.00312362])) np.testing.assert_almost_equal( mus[:, 0], np.array([ 6.86190724e-06, 1.68975654e-01, 8.70683950e-02, 2.47147957e-05, 2.53934274e-05, 8.45479390e-02 ]), ) np.testing.assert_almost_equal( mus[:, -1], np.array([ 1.96717613e-02, 3.42406388e-05, 3.29456098e-05, 8.61728932e-03, 8.57918458e-03, 7.07096066e-03 ]), ) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_symmetry_by_constraint(ode_solver): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "symmetry_by_constraint", str(PROJECT_FOLDER) + "/examples/symmetrical_torque_driven_ocp/symmetry_by_constraint.py", ) symmetry_by_constraint = importlib.util.module_from_spec(spec) spec.loader.exec_module(symmetry_by_constraint) ocp = symmetry_by_constraint.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/symmetrical_torque_driven_ocp/cubeSym.bioMod", ode_solver=ode_solver, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 240.92170742402698) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (276, 1)) np.testing.assert_almost_equal(g, np.zeros((276, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal( q[:, 0], np.array((-0.2, -1.1797959, 0.20135792, -0.20135792))) np.testing.assert_almost_equal( q[:, -1], np.array((0.2, -0.7797959, -0.20135792, 0.20135792))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array((1.16129033, 1.16129033, -0.58458751, 0.58458751))) np.testing.assert_almost_equal( tau[:, -1], np.array((-1.16129033, -1.16129033, 0.58458751, -0.58458751))) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp)
def test_align_segment_on_rt(): # Load align_segment_on_rt PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_segment_on_rt", str(PROJECT_FOLDER) + "/examples/align/align_segment_on_rt.py") align_segment_on_rt = importlib.util.module_from_spec(spec) spec.loader.exec_module(align_segment_on_rt) ocp = align_segment_on_rt.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod", final_time=0.5, number_shooting_points=8, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 197120.95524154368) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (91, 1)) np.testing.assert_almost_equal(g, np.zeros((91, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0.30543155, 0, -1.57, -1.57])) np.testing.assert_almost_equal(q[:, -1], np.array([0.30543155, 0, 1.57, 1.57])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array([0, 9.81, 66.98666900582079, 66.98666424580644])) np.testing.assert_almost_equal( tau[:, -1], np.array([-0, 9.81, -66.98666900582079, -66.98666424580644])) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp)
def test_muscle_activations_with_contact_driven_ocp(): # Load static_arm_with_contact PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "static_arm_with_contact", str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/static_arm_with_contact.py" ) static_arm_with_contact = importlib.util.module_from_spec(spec) spec.loader.exec_module(static_arm_with_contact) ocp = static_arm_with_contact.prepare_ocp( str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26_with_contact.bioMod", final_time=2, number_shooting_points=10, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 0.1435025030068162) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (60, 1)) np.testing.assert_almost_equal(g, np.zeros((60, 1)), decimal=6) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau, mus = states["q"], states["q_dot"], controls["tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0, 0.07, 1.4])) np.testing.assert_almost_equal(q[:, -1], np.array([0.0081671, -0.9450881, 3.0704626])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.0009398, 0.4116121, -0.5587618])) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([-3.9652660e-07, 1.4785825e-03, 5.2079505e-03])) np.testing.assert_almost_equal(tau[:, -1], np.array([-2.7248808e-06, -2.7952503e-04, 6.9262306e-04])) np.testing.assert_almost_equal( mus[:, 0], np.array([2.2873915e-06, 1.6502014e-01, 1.0001872e-01, 4.0192359e-06, 4.1296273e-06, 1.0395487e-01]) ) np.testing.assert_almost_equal( mus[:, -1], np.array([4.2599697e-03, 3.2187363e-05, 3.1307175e-05, 2.0116712e-03, 2.0043861e-03, 1.8230214e-03]) ) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp)
def test_pendulum_min_time_lagrange(ode_solver): # Load pendulum_min_time_Lagrange PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "pendulum_min_time_Lagrange", str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/pendulum_min_time_Lagrange.py", ) pendulum_min_time_Lagrange = importlib.util.module_from_spec(spec) spec.loader.exec_module(pendulum_min_time_Lagrange) ocp = pendulum_min_time_Lagrange.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/pendulum.bioMod", final_time=2, number_shooting_points=10, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 0.062092703196434854) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1)), decimal=6) # Check some of the results states, controls, param = Data.get_data(ocp, sol["x"], get_parameters=True) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] tf = param["time"][0, 0] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((59.9529745, 0))) np.testing.assert_almost_equal(tau[:, -1], np.array((-99.9980341, 0))) # optimized time np.testing.assert_almost_equal(tf, 0.6209270319643485) # save and load TestUtils.save_and_load(sol, ocp, True)
def test_initial_guesses(interpolation): # Load initial_guess PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "initial_guess", str(PROJECT_FOLDER) + "/examples/getting_started/custom_initial_guess.py") initial_guess = importlib.util.module_from_spec(spec) spec.loader.exec_module(initial_guess) np.random.seed(42) ocp = initial_guess.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod", final_time=1, number_shooting_points=5, initial_guess=interpolation, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 13954.735) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (36, 1)) np.testing.assert_almost_equal(g, np.zeros((36, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([1, 0, 0])) np.testing.assert_almost_equal(q[:, -1], np.array([2, 0, 1.57])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([5.0, 9.81, 7.85])) np.testing.assert_almost_equal(tau[:, -1], np.array([-5.0, 9.81, -7.85])) # save and load if interpolation in [InterpolationType.SPLINE, InterpolationType.CUSTOM]: with pytest.raises(AttributeError): TestUtils.save_and_load(sol, ocp, True) else: TestUtils.save_and_load(sol, ocp, True)
def test_align_markers(ode_solver): # Load align_markers PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_markers", str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/align_markers.py") align_markers = importlib.util.module_from_spec(spec) spec.loader.exec_module(align_markers) ocp = align_markers.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod", number_shooting_points=30, final_time=2, use_actuators=False, ode_solver=ode_solver, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 19767.53312569522) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (186, 1)) np.testing.assert_almost_equal(g, np.zeros((186, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array((1.4516128810214546, 9.81, 2.2790322540381487))) np.testing.assert_almost_equal( tau[:, -1], np.array((-1.4516128810214546, 9.81, -2.2790322540381487))) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp)
def test_align_marker_on_segment(): # Load align_marker_on_segment PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_marker_on_segment", str(PROJECT_FOLDER) + "/examples/align/align_marker_on_segment.py") align_marker_on_segment = importlib.util.module_from_spec(spec) spec.loader.exec_module(align_marker_on_segment) ocp = align_marker_on_segment.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod", final_time=0.5, number_shooting_points=8, initialize_near_solution=True, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 42127.04677760122) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (88, 1)) np.testing.assert_almost_equal(g, np.zeros((88, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([1, 0, 0, 0.46364761])) np.testing.assert_almost_equal(q[:, -1], np.array([2, 0, 1.57, 0.78539785])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array([23.6216587, 12.2590703, 31.520697, 12.9472294])) np.testing.assert_almost_equal( tau[:, -1], np.array([-16.659525, 14.5872277, -36.1009998, 4.417834])) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp)
def test_pendulum(nb_threads, use_SX): # Load pendulum PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "pendulum", str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.py") pendulum = importlib.util.module_from_spec(spec) spec.loader.exec_module(pendulum) ocp = pendulum.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.bioMod", final_time=2, number_shooting_points=10, nb_threads=nb_threads, use_SX=use_SX, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 6657.974502951726) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((16.25734477, 0))) np.testing.assert_almost_equal(tau[:, -1], np.array((-25.59944635, 0))) # save and load TestUtils.save_and_load(sol, ocp, True) # simulate TestUtils.simulate(sol, ocp)
def test_cyclic_constraint(): # Load initial_guess PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "initial_guess", str(PROJECT_FOLDER) + "/examples/getting_started/example_cyclic_movement.py") cyclic_movement = importlib.util.module_from_spec(spec) spec.loader.exec_module(cyclic_movement) np.random.seed(42) ocp = cyclic_movement.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod", final_time=1, number_shooting_points=10, loop_from_constraint=True, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 78921.61000000016) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (73, 1)) np.testing.assert_almost_equal(g, np.zeros((73, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([1, 0, 1.57])) np.testing.assert_almost_equal(q[:, -1], np.array([1, 0, 1.57])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([20.0, 9.81, -31.4])) np.testing.assert_almost_equal(tau[:, -1], np.array([20.0, 9.81, -31.4])) # save and load TestUtils.save_and_load(sol, ocp, True) # simulate TestUtils.simulate(sol, ocp)
def test_align_markers_with_actuators(): # Load align_markers PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_markers", str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/align_markers.py") align_markers = importlib.util.module_from_spec(spec) spec.loader.exec_module(align_markers) ocp = align_markers.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod", number_shooting_points=30, final_time=2, use_actuators=True, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 204.18087334169184) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (186, 1)) np.testing.assert_almost_equal(g, np.zeros((186, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0.2140175, 0.981, 0.3360075))) np.testing.assert_almost_equal(tau[:, -1], np.array((-0.2196496, 0.981, -0.3448498))) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol, ocp)
def test_external_forces(): # Load external_forces PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "external_forces", str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/external_forces.py") external_forces = importlib.util.module_from_spec(spec) spec.loader.exec_module(external_forces) ocp = external_forces.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube_with_forces.bioMod", ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 9875.88768746912) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (246, 1)) np.testing.assert_almost_equal(g, np.zeros((246, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((0, 2, 0, 0))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.71322593, 0, 0))) np.testing.assert_almost_equal(tau[:, 10], np.array((0, 7.71100122, 0, 0))) np.testing.assert_almost_equal(tau[:, 20], np.array((0, 5.70877651, 0, 0))) np.testing.assert_almost_equal(tau[:, -1], np.array((0, 3.90677425, 0, 0))) # save and load TestUtils.save_and_load(sol, ocp, True) # simulate TestUtils.simulate(sol, ocp)
def test_maximize_predicted_height_CoM_with_actuators(ode_solver): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "maximize_predicted_height_CoM", str(PROJECT_FOLDER) + "/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py", ) maximize_predicted_height_CoM = importlib.util.module_from_spec(spec) spec.loader.exec_module(maximize_predicted_height_CoM) ocp = maximize_predicted_height_CoM.prepare_ocp( model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_with_contact/2segments_4dof_2contacts.bioMod", phase_time=0.5, number_shooting_points=20, use_actuators=True, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 0.21850679397314332) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (160, 1)) np.testing.assert_almost_equal(g, np.zeros((160, 1)), decimal=6) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0.0, 0.0, -0.5, 0.5))) np.testing.assert_almost_equal(q[:, -1], np.array((-0.2393758, 0.0612086, -0.0006739, 0.0006739))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal( qdot[:, -1], np.array((-4.8768219e-01, 3.2867302e-04, 9.7536459e-01, -9.7536459e-01)) ) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-0.550905))) np.testing.assert_almost_equal(tau[:, -1], np.array(-0.0050623)) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_custom_problem_type_and_dynamics(problem_type_custom): # Load pendulum PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "custom_problem_type_and_dynamics", str(PROJECT_FOLDER) + "/examples/getting_started/custom_dynamics.py", ) pendulum = importlib.util.module_from_spec(spec) spec.loader.exec_module(pendulum) ocp = pendulum.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod", problem_type_custom=problem_type_custom, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 19767.5331257) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (186, 1)) np.testing.assert_almost_equal(g, np.zeros((186, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((1, 0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((1.4516129, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -1], np.array((-1.45161291, 9.81, -2.27903226)))
def test_maximize_predicted_height_CoM(ode_solver): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "maximize_predicted_height_CoM", str(PROJECT_FOLDER) + "/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py", ) maximize_predicted_height_CoM = importlib.util.module_from_spec(spec) spec.loader.exec_module(maximize_predicted_height_CoM) ocp = maximize_predicted_height_CoM.prepare_ocp( model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_with_contact/2segments_4dof_2contacts.bioMod", phase_time=0.5, number_shooting_points=20, use_actuators=False, ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 0.7592028279017864) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (160, 1)) np.testing.assert_almost_equal(g, np.zeros((160, 1))) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0.0, 0.0, -0.5, 0.5))) np.testing.assert_almost_equal(q[:, -1], np.array((0.1189651, -0.0904378, -0.7999996, 0.7999996))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((1.2636414, -1.3010929, -3.6274687, 3.6274687))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-22.1218282))) np.testing.assert_almost_equal(tau[:, -1], np.array(0.2653957)) # save and load TestUtils.save_and_load(sol, ocp, False)
def test_simulate_from_initial_single_shoot(): # Load pendulum PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "pendulum", str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.py") pendulum = importlib.util.module_from_spec(spec) spec.loader.exec_module(pendulum) ocp = pendulum.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.bioMod", final_time=2, number_shooting_points=10, nb_threads=4, ) X = InitialConditions([-1, -2, 1, 0.5]) U = InitialConditions(np.array([[-0.1, 0], [1, 2]]).T, interpolation=InterpolationType.LINEAR) sol_simulate_single_shooting = Simulate.from_controls_and_initial_states( ocp, X, U, single_shoot=True) # Check some of the results states, controls = Data.get_data(ocp, sol_simulate_single_shooting["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0))) np.testing.assert_almost_equal(q[:, -1], np.array( (-0.59371229, 2.09731719))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5))) np.testing.assert_almost_equal(qdot[:, -1], np.array((1.38153013, -0.60425128))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0))) np.testing.assert_almost_equal(tau[:, -1], np.array((1.0, 2.0)))