def test_update_bounds_and_init_with_param(): def my_parameter_function(biorbd_model, value, extra_value): biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value)) def my_target_function(ocp, value, target_value): return value + target_value biorbd_model = biorbd.Model(TestUtils.bioptim_folder() + "/examples/track/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 g_min, g_max, g_init = -10, -6, -8 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) parameters = ParameterList() bounds_gravity = Bounds(g_min, g_max, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess(g_init) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=-8 ) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bounds_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0, parameters=parameters) x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))) u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds, u_bounds) expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, np.append(expected, [g_min])[:, np.newaxis]) expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, np.append(expected, [g_max])[:, np.newaxis]) x_init = InitialGuess(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, np.append(expected, [g_init])[:, np.newaxis])
def test_double_update_bounds_and_init(): bioptim_folder = TestUtils.bioptim_folder() biorbd_model = biorbd.Model(bioptim_folder + "/examples/track/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0) x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))) u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds, u_bounds) expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, expected) expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, expected) x_init = InitialGuess(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, expected) x_bounds = Bounds(-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1))) u_bounds = Bounds(-4.0 * np.ones((nq, 1)), 4.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds=x_bounds) ocp.update_bounds(u_bounds=u_bounds) expected = np.array([[-2] * (nq * 2) * (ns + 1) + [-4] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, expected) expected = np.array([[2] * (nq * 2) * (ns + 1) + [4] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, expected) x_init = InitialGuess(0.25 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.25 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.25] * (nq * 2) * (ns + 1) + [-0.25] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, expected) with pytest.raises( RuntimeError, match= "x_init should be built from a InitialGuess or InitialGuessList"): ocp.update_initial_guess(x_bounds, u_bounds) with pytest.raises( RuntimeError, match="x_bounds should be built from a Bounds or BoundsList"): ocp.update_bounds(x_init, u_init)
def test_update_bounds_and_init_with_param(): def my_parameter_function(biorbd_model, value, extra_value): biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value)) def my_target_function(ocp, value, target_value): return value + target_value PROJECT_FOLDER = Path(__file__).parent / ".." biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 g_min, g_max, g_init = -10, -6, -8 dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) parameters = ParameterList() bounds_gravity = Bounds(min_bound=g_min, max_bound=g_max, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess(g_init) parameter_objective_functions = ObjectiveOption( my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target_value=-8) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bounds_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0, parameters=parameters) x_bounds = BoundsOption([-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))]) u_bounds = BoundsOption([-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))]) ocp.update_bounds(x_bounds, u_bounds) expected = np.append( np.tile(np.append(-np.ones((nq * 2, 1)), -2.0 * np.ones((nq, 1))), ns), -np.ones((nq * 2, 1))) np.testing.assert_almost_equal( ocp.V_bounds.min, np.append([g_min], expected).reshape(129, 1)) expected = np.append( np.tile(np.append(np.ones((nq * 2, 1)), 2.0 * np.ones((nq, 1))), ns), np.ones((nq * 2, 1))) np.testing.assert_almost_equal( ocp.V_bounds.max, np.append([[g_max]], expected).reshape(129, 1)) x_init = InitialGuessOption(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuessOption(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.append( np.tile(np.append(0.5 * np.ones((nq * 2, 1)), -0.5 * np.ones((nq, 1))), ns), 0.5 * np.ones((nq * 2, 1))) np.testing.assert_almost_equal( ocp.V_init.init, np.append([g_init], expected).reshape(129, 1))
def test_double_update_bounds_and_init(): PROJECT_FOLDER = Path(__file__).parent / ".." biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0) x_bounds = BoundsOption([-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))]) u_bounds = BoundsOption([-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))]) ocp.update_bounds(x_bounds, u_bounds) expected = np.append( np.tile(np.append(-np.ones((nq * 2, 1)), -2.0 * np.ones((nq, 1))), ns), -np.ones((nq * 2, 1))) np.testing.assert_almost_equal(ocp.V_bounds.min, expected.reshape(128, 1)) expected = np.append( np.tile(np.append(np.ones((nq * 2, 1)), 2.0 * np.ones((nq, 1))), ns), np.ones((nq * 2, 1))) np.testing.assert_almost_equal(ocp.V_bounds.max, expected.reshape(128, 1)) x_init = InitialGuessOption(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuessOption(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.append( np.tile(np.append(0.5 * np.ones((nq * 2, 1)), -0.5 * np.ones((nq, 1))), ns), 0.5 * np.ones((nq * 2, 1))) np.testing.assert_almost_equal(ocp.V_init.init, expected.reshape(128, 1)) x_bounds = BoundsOption( [-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1))]) u_bounds = BoundsOption([-4.0 * np.ones((nq, 1)), 4.0 * np.ones((nq, 1))]) ocp.update_bounds(x_bounds=x_bounds) ocp.update_bounds(u_bounds=u_bounds) expected = np.append( np.tile( np.append(-2.0 * np.ones((nq * 2, 1)), -4.0 * np.ones((nq, 1))), ns), -2.0 * np.ones((nq * 2, 1))) np.testing.assert_almost_equal(ocp.V_bounds.min, expected.reshape(128, 1)) expected = np.append( np.tile(np.append(2.0 * np.ones((nq * 2, 1)), 4.0 * np.ones((nq, 1))), ns), 2.0 * np.ones((nq * 2, 1))) np.testing.assert_almost_equal(ocp.V_bounds.max, expected.reshape(128, 1)) x_init = InitialGuessOption(0.25 * np.ones((nq * 2, 1))) u_init = InitialGuessOption(-0.25 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.append( np.tile( np.append(0.25 * np.ones((nq * 2, 1)), -0.25 * np.ones((nq, 1))), ns), 0.25 * np.ones((nq * 2, 1))) np.testing.assert_almost_equal(ocp.V_init.init, expected.reshape(128, 1)) with pytest.raises( RuntimeError, match= "x_init should be built from a InitialGuessOption or InitialGuessList" ): ocp.update_initial_guess(x_bounds, u_bounds) with pytest.raises( RuntimeError, match="x_bounds should be built from a BoundsOption or BoundsList" ): ocp.update_bounds(x_init, u_init)