Exemple #1
0
def test_implicit_dynamics_errors(dynamics):
    # Prepare the program
    nlp = NonLinearProgram()
    nlp.model = biorbd.Model(
        TestUtils.bioptim_folder() +
        "/examples/getting_started/models/2segments_4dof_2contacts.bioMod")
    nlp.ns = 5
    nlp.cx = MX

    nlp.u_bounds = np.zeros((nlp.model.nbQ() * 4, 1))
    ocp = OptimalControlProgram(nlp)
    nlp.control_type = ControlType.CONSTANT
    NonLinearProgram.add(
        ocp,
        "dynamics_type",
        Dynamics(dynamics, implicit_dynamics=True),
        False,
    )

    # Prepare the dynamics
    with pytest.raises(
            TypeError,
            match=re.escape(
                f"{dynamics.name.lower()}() got an unexpected keyword argument "
                "'implicit_dynamics'"),
    ):
        ConfigureProblem.initialize(ocp, nlp)
Exemple #2
0
def test_custom_dynamics(with_contact):
    def custom_dynamic(states, controls, parameters, nlp, with_contact=False) -> tuple:
        DynamicsFunctions.apply_parameters(parameters, nlp)
        q = DynamicsFunctions.get(nlp.states["q"], states)
        qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
        tau = DynamicsFunctions.get(nlp.controls["tau"], controls)

        dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
        ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact)

        return dq, ddq

    def configure(ocp, nlp, with_contact=None):
        ConfigureProblem.configure_q(nlp, True, False)
        ConfigureProblem.configure_qdot(nlp, True, False)
        ConfigureProblem.configure_tau(nlp, False, True)
        ConfigureProblem.configure_dynamics_function(ocp, nlp, custom_dynamic, with_contact=with_contact)

        if with_contact:
            ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven)

    # Prepare the program
    nlp = NonLinearProgram()
    nlp.model = biorbd.Model(
        TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
    )
    nlp.ns = 5
    nlp.cx = MX

    nlp.x_bounds = np.zeros((nlp.model.nbQ() * 3, 1))
    nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1))
    ocp = OptimalControlProgram(nlp)
    nlp.control_type = ControlType.CONSTANT
    NonLinearProgram.add(
        ocp, "dynamics_type", Dynamics(configure, dynamic_function=custom_dynamic, with_contact=with_contact), False
    )

    np.random.seed(42)

    # Prepare the dynamics
    ConfigureProblem.initialize(ocp, nlp)

    # Test the results
    states = np.random.rand(nlp.states.shape, nlp.ns)
    controls = np.random.rand(nlp.controls.shape, nlp.ns)
    params = np.random.rand(nlp.parameters.shape, nlp.ns)
    x_out = np.array(nlp.dynamics_func(states, controls, params))

    if with_contact:
        contact_out = np.array(nlp.contact_forces_func(states, controls, params))
        np.testing.assert_almost_equal(
            x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716]
        )
        np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124])

    else:
        np.testing.assert_almost_equal(
            x_out[:, 0],
            [0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642],
        )
Exemple #3
0
def test_torque_driven_implicit_soft_contacts(with_contact, cx,
                                              implicit_contact):
    # Prepare the program
    nlp = NonLinearProgram()
    nlp.model = biorbd.Model(
        TestUtils.bioptim_folder() +
        "/examples/getting_started/models/2segments_4dof_2contacts.bioMod")
    nlp.ns = 5
    nlp.cx = cx

    nlp.x_bounds = np.zeros((nlp.model.nbQ() * (2 + 3), 1))
    nlp.u_bounds = np.zeros((nlp.model.nbQ() * 2, 1))
    ocp = OptimalControlProgram(nlp)
    nlp.control_type = ControlType.CONSTANT
    nlp.phase_idx = 0

    NonLinearProgram.add(
        ocp,
        "dynamics_type",
        Dynamics(DynamicsFcn.TORQUE_DRIVEN,
                 with_contact=with_contact,
                 implicit_soft_contacts=implicit_contact),
        False,
    )

    # Prepare the dynamics
    ConfigureProblem.initialize(ocp, nlp)

    # Test the results
    np.random.seed(42)
    states = np.random.rand(nlp.states.shape, nlp.ns)
    controls = np.random.rand(nlp.controls.shape, nlp.ns)
    params = np.random.rand(nlp.parameters.shape, nlp.ns)
    x_out = np.array(nlp.dynamics_func(states, controls, params))

    if with_contact:
        contact_out = np.array(
            nlp.contact_forces_func(states, controls, params))
        np.testing.assert_almost_equal(x_out[:, 0], [
            0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131,
            0.6507164, -0.2359716
        ])

        np.testing.assert_almost_equal(contact_out[:, 0],
                                       [-2.444071, 128.8816865, 2.7245124])

    else:
        np.testing.assert_almost_equal(
            x_out[:, 0],
            [
                0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3024137,
                -10.3850379, 1.6044517, 35.8023864
            ],
        )
Exemple #4
0
def test_torque_driven(with_contact, with_external_force, cx):
    # Prepare the program
    nlp = NonLinearProgram()
    nlp.model = biorbd.Model(
        TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
    )
    nlp.ns = 5
    nlp.cx = cx

    nlp.x_bounds = np.zeros((nlp.model.nbQ() * 3, 1))
    nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1))
    ocp = OptimalControlProgram(nlp)
    nlp.control_type = ControlType.CONSTANT
    NonLinearProgram.add(ocp, "dynamics_type", Dynamics(DynamicsFcn.TORQUE_DRIVEN, with_contact=with_contact), False)

    np.random.seed(42)
    if with_external_force:
        external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)]
        nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0]

    # Prepare the dynamics
    ConfigureProblem.initialize(ocp, nlp)

    # Test the results
    states = np.random.rand(nlp.states.shape, nlp.ns)
    controls = np.random.rand(nlp.controls.shape, nlp.ns)
    params = np.random.rand(nlp.parameters.shape, nlp.ns)
    x_out = np.array(nlp.dynamics_func(states, controls, params))

    if with_contact:
        contact_out = np.array(nlp.contact_forces_func(states, controls, params))
        if with_external_force:
            np.testing.assert_almost_equal(
                x_out[:, 0],
                [0.8631034, 0.3251833, 0.1195942, 0.4937956, -7.7700092, -7.5782306, 21.7073786, -16.3059315],
            )
            np.testing.assert_almost_equal(contact_out[:, 0], [-47.8131136, 111.1726516, -24.4449121])
        else:
            np.testing.assert_almost_equal(
                x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716]
            )
            np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124])

    else:
        if with_external_force:
            np.testing.assert_almost_equal(
                x_out[:, 0],
                [0.86310343, 0.32518332, 0.11959425, 0.4937956, 0.30731739, -9.97912778, 1.15263778, 36.02430956],
            )
        else:
            np.testing.assert_almost_equal(
                x_out[:, 0],
                [0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642],
            )
Exemple #5
0
def test_torque_derivative_driven_implicit(with_contact, cx):
    # Prepare the program
    nlp = NonLinearProgram()
    nlp.model = biorbd.Model(
        TestUtils.bioptim_folder() +
        "/examples/getting_started/models/2segments_4dof_2contacts.bioMod")
    nlp.ns = 5
    nlp.cx = cx
    nlp.phase_idx = 0
    nlp.x_bounds = np.zeros((nlp.model.nbQ() * 4, 1))
    nlp.u_bounds = np.zeros((nlp.model.nbQ(), 2))
    ocp = OptimalControlProgram(nlp)
    nlp.control_type = ControlType.CONSTANT
    NonLinearProgram.add(
        ocp,
        "dynamics_type",
        Dynamics(DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN,
                 with_contact=with_contact,
                 implicit_dynamics=True),
        False,
    )

    # Prepare the dynamics
    ConfigureProblem.initialize(ocp, nlp)

    # Test the results
    np.random.seed(42)
    states = np.random.rand(nlp.states.shape, nlp.ns)
    controls = np.random.rand(nlp.controls.shape, nlp.ns)
    params = np.random.rand(nlp.parameters.shape, nlp.ns)
    x_out = np.array(nlp.dynamics_func(states, controls, params))

    if with_contact:
        contact_out = np.array(
            nlp.contact_forces_func(states, controls, params))
        np.testing.assert_almost_equal(
            x_out[:, 0],
            [
                0.6118529,
                0.785176,
                0.6075449,
                0.8083973,
                0.3886773,
                0.5426961,
                0.7722448,
                0.7290072,
                0.8631034,
                0.3251833,
                0.1195942,
                0.4937956,
                0.0314292,
                0.2492922,
                0.2897515,
                0.8714606,
            ],
        )
        np.testing.assert_almost_equal(contact_out[:, 0],
                                       [-2.444071, 128.8816865, 2.7245124])
    else:
        np.testing.assert_almost_equal(
            x_out[:, 0],
            [
                0.6118529,
                0.785176,
                0.6075449,
                0.8083973,
                0.3886773,
                0.5426961,
                0.7722448,
                0.7290072,
                0.8631034,
                0.3251833,
                0.1195942,
                0.4937956,
                0.0314292,
                0.2492922,
                0.2897515,
                0.8714606,
            ],
        )
Exemple #6
0
def test_muscle_driven(with_excitations, with_contact, with_residual_torque, with_external_force, cx):
    # Prepare the program
    nlp = NonLinearProgram()
    nlp.model = biorbd.Model(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/arm26_with_contact.bioMod")
    nlp.ns = 5
    nlp.cx = cx

    nlp.x_bounds = np.zeros((nlp.model.nbQ() * 2 + nlp.model.nbMuscles(), 1))
    nlp.u_bounds = np.zeros((nlp.model.nbMuscles(), 1))
    ocp = OptimalControlProgram(nlp)
    nlp.control_type = ControlType.CONSTANT
    NonLinearProgram.add(
        ocp,
        "dynamics_type",
        Dynamics(
            DynamicsFcn.MUSCLE_DRIVEN,
            with_residual_torque=with_residual_torque,
            with_excitations=with_excitations,
            with_contact=with_contact,
        ),
        False,
    )

    np.random.seed(42)
    if with_external_force:
        external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)]
        nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0]

    # Prepare the dynamics
    ConfigureProblem.initialize(ocp, nlp)

    # Test the results
    states = np.random.rand(nlp.states.shape, nlp.ns)
    controls = np.random.rand(nlp.controls.shape, nlp.ns)
    params = np.random.rand(nlp.parameters.shape, nlp.ns)
    x_out = np.array(nlp.dynamics_func(states, controls, params))

    if with_contact:  # Warning this test is a bit bogus, there since the model does not have contacts
        if with_residual_torque:
            if with_excitations:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            0.6158501,
                            0.50313626,
                            0.64241928,
                            1.07179591,
                            -33.76216759,
                            36.218136,
                            46.87928022,
                            -1.80189035,
                            53.3914525,
                            48.30056919,
                            63.69373374,
                            -28.15700995,
                        ],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            1.83404510e-01,
                            6.11852895e-01,
                            7.85175961e-01,
                            -9.29661929e00,
                            3.00871754e02,
                            -9.50353966e02,
                            8.60630831e00,
                            3.19433638e00,
                            2.97405608e01,
                            -2.02754226e01,
                            -2.32467778e01,
                            -4.19135012e01,
                        ],
                        decimal=6,
                    )
            else:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -8.06477702e00, 2.42278904e02, -7.72113478e02],
                        decimal=6,
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.80891773e00, 1.20475911e02, -4.33290914e02],
                        decimal=6,
                    )

        else:
            if with_excitations:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            0.6158501,
                            0.50313626,
                            0.64241928,
                            0.91952666,
                            -39.04874824,
                            45.31834295,
                            55.65557816,
                            50.47052688,
                            0.36025589,
                            58.92377491,
                            29.70094194,
                            -15.13534937,
                        ],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            1.83404510e-01,
                            6.11852895e-01,
                            7.85175961e-01,
                            -9.72711369e00,
                            3.10865851e02,
                            -9.82724687e02,
                            -7.72228930e00,
                            -1.13759732e01,
                            9.51906209e01,
                            4.45077128e00,
                            -5.20261014e00,
                            -2.80864106e01,
                        ],
                        decimal=6,
                    )
            else:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [0.6158501, 0.50313626, 0.64241928, 0.91952666, -39.04874824, 45.31834295],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.72711369e00, 3.10865851e02, -9.82724687e02],
                        decimal=6,
                    )

    else:
        if with_residual_torque:
            if with_excitations:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            0.6158501,
                            0.50313626,
                            0.64241928,
                            1.07179591,
                            -33.76216759,
                            36.218136,
                            46.87928022,
                            -1.80189035,
                            53.3914525,
                            48.30056919,
                            63.69373374,
                            -28.15700995,
                        ],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            1.83404510e-01,
                            6.11852895e-01,
                            7.85175961e-01,
                            -9.29661929e00,
                            3.00871754e02,
                            -9.50353966e02,
                            8.60630831e00,
                            3.19433638e00,
                            2.97405608e01,
                            -2.02754226e01,
                            -2.32467778e01,
                            -4.19135012e01,
                        ],
                        decimal=6,
                    )
            else:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -8.06477702e00, 2.42278904e02, -7.72113478e02],
                        decimal=6,
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.80891773e00, 1.20475911e02, -4.33290914e02],
                        decimal=6,
                    )

        else:
            if with_excitations:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            0.6158501,
                            0.5031363,
                            0.6424193,
                            0.9195267,
                            -39.0487482,
                            45.3183429,
                            55.6555782,
                            50.4705269,
                            0.3602559,
                            58.9237749,
                            29.7009419,
                            -15.1353494,
                        ],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            1.83404510e-01,
                            6.11852895e-01,
                            7.85175961e-01,
                            -9.72711369e00,
                            3.10865851e02,
                            -9.82724687e02,
                            -7.72228930e00,
                            -1.13759732e01,
                            9.51906209e01,
                            4.45077128e00,
                            -5.20261014e00,
                            -2.80864106e01,
                        ],
                        decimal=6,
                    )
            else:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [0.6158501, 0.50313626, 0.64241928, 0.91952666, -39.04874824, 45.31834295],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.72711369e00, 3.10865851e02, -9.82724687e02],
                        decimal=6,
                    )