import pytest

import numpy as np
from casadi import MX, SX
import biorbd_casadi as biorbd
from bioptim.dynamics.configure_problem import ConfigureProblem
from bioptim.dynamics.dynamics_functions import DynamicsFunctions
from bioptim.interfaces.biorbd_interface import BiorbdInterface
from bioptim.misc.enums import ControlType
from bioptim.optimization.non_linear_program import NonLinearProgram
from bioptim.optimization.optimization_vector import OptimizationVector
from bioptim.dynamics.configure_problem import DynamicsFcn, Dynamics

from .utils import TestUtils


class OptimalControlProgram:
    def __init__(self, nlp):
        self.n_phases = 1
        self.nlp = [nlp]
        self.v = OptimizationVector(self)


@pytest.mark.parametrize("cx", [MX, SX])
@pytest.mark.parametrize("with_external_force", [False, True])
@pytest.mark.parametrize("with_contact", [False, True])
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],
            )


@pytest.mark.parametrize("cx", [MX, SX])
@pytest.mark.parametrize("with_external_force", [False, True])
@pytest.mark.parametrize("with_contact", [False, True])
def test_torque_derivative_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_DERIVATIVE_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,
                    0.8074402,
                    0.4271078,
                    0.417411,
                    0.3232029,
                ],
            )
            np.testing.assert_almost_equal(contact_out[:, 0], [-47.8131136, 111.1726516, -24.4449121])
        else:
            np.testing.assert_almost_equal(
                x_out[:, 0],
                [
                    0.61185289,
                    0.78517596,
                    0.60754485,
                    0.80839735,
                    -0.32149054,
                    -0.19121314,
                    0.65071636,
                    -0.23597164,
                    0.38867729,
                    0.54269608,
                    0.77224477,
                    0.72900717,
                ],
            )
            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,
                    0.80744016,
                    0.42710779,
                    0.417411,
                    0.32320293,
                ],
            )
        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,
                    0.38867729,
                    0.54269608,
                    0.77224477,
                    0.72900717,
                ],
            )


@pytest.mark.parametrize("cx", [MX, SX])
@pytest.mark.parametrize("with_external_force", [False, True])
@pytest.mark.parametrize("with_contact", [False, True])
def test_torque_activation_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() * 2, 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_ACTIVATIONS_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.8631, 0.32518, 0.11959, 0.4938, 19.01887, 18.51503, -53.08574, 58.48719],
                decimal=5,
            )
            np.testing.assert_almost_equal(contact_out[:, 0], [109.8086936, 3790.3932439, -3571.7858574])
        else:
            np.testing.assert_almost_equal(
                x_out[:, 0],
                [0.61185289, 0.78517596, 0.60754485, 0.80839735, 0.78455384, -0.16844256, -1.56184114, 1.97658587],
                decimal=5,
            )
            np.testing.assert_almost_equal(contact_out[:, 0], [-7.88958997, 329.70828173, -263.55516549])

    else:
        if with_external_force:
            np.testing.assert_almost_equal(
                x_out[:, 0],
                [
                    8.63103426e-01,
                    3.25183322e-01,
                    1.19594246e-01,
                    4.93795596e-01,
                    1.73558072e01,
                    -4.69891264e01,
                    1.81396922e02,
                    3.61170139e03,
                ],
                decimal=5,
            )
        else:
            np.testing.assert_almost_equal(
                x_out[:, 0],
                [
                    6.11852895e-01,
                    7.85175961e-01,
                    6.07544852e-01,
                    8.08397348e-01,
                    -2.38262975e01,
                    -5.82033454e01,
                    1.27439020e02,
                    3.66531163e03,
                ],
                decimal=5,
            )


@pytest.mark.parametrize("cx", [MX, SX])
@pytest.mark.parametrize("with_external_force", [False, True])
@pytest.mark.parametrize("with_contact", [False, True])
@pytest.mark.parametrize("with_torque", [False, True])
@pytest.mark.parametrize("with_excitations", [False, True])
def test_muscle_driven(with_excitations, with_contact, with_torque, with_external_force, cx):
    # Prepare the program
    nlp = NonLinearProgram()
    nlp.model = biorbd.Model(
        TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/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_torque=with_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_torque:
            if with_excitations:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            0.6158501,
                            0.50313626,
                            0.64241928,
                            1.07179622,
                            -33.76217857,
                            36.21815923,
                            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.29662878e00,
                            3.00872062e02,
                            -9.50354903e02,
                            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.06478367e00, 2.42279101e02, -7.72114103e02],
                        decimal=6,
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.80892207e00, 1.20476051e02, -4.33291346e02],
                        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.91952705,
                            -39.04876174,
                            45.31837288,
                            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.72712350e00,
                            3.10866170e02,
                            -9.82725656e02,
                            -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.91952705, -39.04876174, 45.31837288],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.72712350e00, 3.10866170e02, -9.82725656e02],
                        decimal=6,
                    )

    else:
        if with_torque:
            if with_excitations:
                if with_external_force:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [
                            0.6158501,
                            0.50313626,
                            0.64241928,
                            1.07179622,
                            -33.76217857,
                            36.21815923,
                            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.29662878e00,
                            3.00872062e02,
                            -9.50354903e02,
                            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.06478367e00, 2.42279101e02, -7.72114103e02],
                        decimal=6,
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.80892207e00, 1.20476051e02, -4.33291346e02],
                        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.91952705,
                            -39.04876174,
                            45.31837288,
                            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.72712350e00,
                            3.10866170e02,
                            -9.82725656e02,
                            -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.91952705, -39.04876174, 45.31837288],
                    )
                else:
                    np.testing.assert_almost_equal(
                        x_out[:, 0],
                        [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.72712350e00, 3.10866170e02, -9.82725656e02],
                        decimal=6,
                    )


@pytest.mark.parametrize("with_contact", [False, True])
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],
        )

[((24, 1, 24, 40), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(24, 25, 24, 29): '"""cx"""', (24, 31, 24, 39): '[MX, SX]'}, {}), "('cx', [MX, SX])", False, 'import pytest\n'), ((25, 1, 25, 62), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(25, 25, 25, 46): '"""with_external_force"""', (25, 48, 25, 61): '[False, True]'}, {}), "('with_external_force', [False, True])", False, 'import pytest\n'), ((26, 1, 26, 55), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(26, 25, 26, 39): '"""with_contact"""', (26, 41, 26, 54): '[False, True]'}, {}), "('with_contact', [False, True])", False, 'import pytest\n'), ((83, 1, 83, 40), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(83, 25, 83, 29): '"""cx"""', (83, 31, 83, 39): '[MX, SX]'}, {}), "('cx', [MX, SX])", False, 'import pytest\n'), ((84, 1, 84, 62), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(84, 25, 84, 46): '"""with_external_force"""', (84, 48, 84, 61): '[False, True]'}, {}), "('with_external_force', [False, True])", False, 'import pytest\n'), ((85, 1, 85, 55), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(85, 25, 85, 39): '"""with_contact"""', (85, 41, 85, 54): '[False, True]'}, {}), "('with_contact', [False, True])", False, 'import pytest\n'), ((197, 1, 197, 40), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(197, 25, 197, 29): '"""cx"""', (197, 31, 197, 39): '[MX, SX]'}, {}), "('cx', [MX, SX])", False, 'import pytest\n'), ((198, 1, 198, 62), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(198, 25, 198, 46): '"""with_external_force"""', (198, 48, 198, 61): '[False, True]'}, {}), "('with_external_force', [False, True])", False, 'import pytest\n'), ((199, 1, 199, 55), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(199, 25, 199, 39): '"""with_contact"""', (199, 41, 199, 54): '[False, True]'}, {}), "('with_contact', [False, True])", False, 'import pytest\n'), ((280, 1, 280, 40), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(280, 25, 280, 29): '"""cx"""', (280, 31, 280, 39): '[MX, SX]'}, {}), "('cx', [MX, SX])", False, 'import pytest\n'), ((281, 1, 281, 62), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(281, 25, 281, 46): '"""with_external_force"""', (281, 48, 281, 61): '[False, True]'}, {}), "('with_external_force', [False, True])", False, 'import pytest\n'), ((282, 1, 282, 55), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(282, 25, 282, 39): '"""with_contact"""', (282, 41, 282, 54): '[False, True]'}, {}), "('with_contact', [False, True])", False, 'import pytest\n'), ((283, 1, 283, 54), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(283, 25, 283, 38): '"""with_torque"""', (283, 40, 283, 53): '[False, True]'}, {}), "('with_torque', [False, True])", False, 'import pytest\n'), ((284, 1, 284, 59), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(284, 25, 284, 43): '"""with_excitations"""', (284, 45, 284, 58): '[False, True]'}, {}), "('with_excitations', [False, True])", False, 'import pytest\n'), ((537, 1, 537, 55), 'pytest.mark.parametrize', 'pytest.mark.parametrize', ({(537, 25, 537, 39): '"""with_contact"""', (537, 41, 537, 54): '[False, True]'}, {}), "('with_contact', [False, True])", False, 'import pytest\n'), ((29, 10, 29, 28), 'bioptim.optimization.non_linear_program.NonLinearProgram', 'NonLinearProgram', ({}, {}), '()', False, 'from bioptim.optimization.non_linear_program import NonLinearProgram\n'), ((42, 4, 42, 22), 'numpy.random.seed', 'np.random.seed', ({(42, 19, 42, 21): '(42)'}, {}), '(42)', True, 'import numpy as np\n'), ((48, 4, 48, 41), 'bioptim.dynamics.configure_problem.ConfigureProblem.initialize', 'ConfigureProblem.initialize', ({(48, 32, 48, 35): 'ocp', (48, 37, 48, 40): 'nlp'}, {}), '(ocp, nlp)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((51, 13, 51, 53), 'numpy.random.rand', 'np.random.rand', ({(51, 28, 51, 44): 'nlp.states.shape', (51, 46, 51, 52): 'nlp.ns'}, {}), '(nlp.states.shape, nlp.ns)', True, 'import numpy as np\n'), ((52, 15, 52, 57), 'numpy.random.rand', 'np.random.rand', ({(52, 30, 52, 48): 'nlp.controls.shape', (52, 50, 52, 56): 'nlp.ns'}, {}), '(nlp.controls.shape, nlp.ns)', True, 'import numpy as np\n'), ((53, 13, 53, 57), 'numpy.random.rand', 'np.random.rand', ({(53, 28, 53, 48): 'nlp.parameters.shape', (53, 50, 53, 56): 'nlp.ns'}, {}), '(nlp.parameters.shape, nlp.ns)', True, 'import numpy as np\n'), ((88, 10, 88, 28), 'bioptim.optimization.non_linear_program.NonLinearProgram', 'NonLinearProgram', ({}, {}), '()', False, 'from bioptim.optimization.non_linear_program import NonLinearProgram\n'), ((103, 4, 103, 22), 'numpy.random.seed', 'np.random.seed', ({(103, 19, 103, 21): '(42)'}, {}), '(42)', True, 'import numpy as np\n'), ((109, 4, 109, 41), 'bioptim.dynamics.configure_problem.ConfigureProblem.initialize', 'ConfigureProblem.initialize', ({(109, 32, 109, 35): 'ocp', (109, 37, 109, 40): 'nlp'}, {}), '(ocp, nlp)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((112, 13, 112, 53), 'numpy.random.rand', 'np.random.rand', ({(112, 28, 112, 44): 'nlp.states.shape', (112, 46, 112, 52): 'nlp.ns'}, {}), '(nlp.states.shape, nlp.ns)', True, 'import numpy as np\n'), ((113, 15, 113, 57), 'numpy.random.rand', 'np.random.rand', ({(113, 30, 113, 48): 'nlp.controls.shape', (113, 50, 113, 56): 'nlp.ns'}, {}), '(nlp.controls.shape, nlp.ns)', True, 'import numpy as np\n'), ((114, 13, 114, 57), 'numpy.random.rand', 'np.random.rand', ({(114, 28, 114, 48): 'nlp.parameters.shape', (114, 50, 114, 56): 'nlp.ns'}, {}), '(nlp.parameters.shape, nlp.ns)', True, 'import numpy as np\n'), ((202, 10, 202, 28), 'bioptim.optimization.non_linear_program.NonLinearProgram', 'NonLinearProgram', ({}, {}), '()', False, 'from bioptim.optimization.non_linear_program import NonLinearProgram\n'), ((216, 4, 216, 22), 'numpy.random.seed', 'np.random.seed', ({(216, 19, 216, 21): '(42)'}, {}), '(42)', True, 'import numpy as np\n'), ((222, 4, 222, 41), 'bioptim.dynamics.configure_problem.ConfigureProblem.initialize', 'ConfigureProblem.initialize', ({(222, 32, 222, 35): 'ocp', (222, 37, 222, 40): 'nlp'}, {}), '(ocp, nlp)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((225, 13, 225, 53), 'numpy.random.rand', 'np.random.rand', ({(225, 28, 225, 44): 'nlp.states.shape', (225, 46, 225, 52): 'nlp.ns'}, {}), '(nlp.states.shape, nlp.ns)', True, 'import numpy as np\n'), ((226, 15, 226, 57), 'numpy.random.rand', 'np.random.rand', ({(226, 30, 226, 48): 'nlp.controls.shape', (226, 50, 226, 56): 'nlp.ns'}, {}), '(nlp.controls.shape, nlp.ns)', True, 'import numpy as np\n'), ((227, 13, 227, 57), 'numpy.random.rand', 'np.random.rand', ({(227, 28, 227, 48): 'nlp.parameters.shape', (227, 50, 227, 56): 'nlp.ns'}, {}), '(nlp.parameters.shape, nlp.ns)', True, 'import numpy as np\n'), ((287, 10, 287, 28), 'bioptim.optimization.non_linear_program.NonLinearProgram', 'NonLinearProgram', ({}, {}), '()', False, 'from bioptim.optimization.non_linear_program import NonLinearProgram\n'), ((310, 4, 310, 22), 'numpy.random.seed', 'np.random.seed', ({(310, 19, 310, 21): '(42)'}, {}), '(42)', True, 'import numpy as np\n'), ((316, 4, 316, 41), 'bioptim.dynamics.configure_problem.ConfigureProblem.initialize', 'ConfigureProblem.initialize', ({(316, 32, 316, 35): 'ocp', (316, 37, 316, 40): 'nlp'}, {}), '(ocp, nlp)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((319, 13, 319, 53), 'numpy.random.rand', 'np.random.rand', ({(319, 28, 319, 44): 'nlp.states.shape', (319, 46, 319, 52): 'nlp.ns'}, {}), '(nlp.states.shape, nlp.ns)', True, 'import numpy as np\n'), ((320, 15, 320, 57), 'numpy.random.rand', 'np.random.rand', ({(320, 30, 320, 48): 'nlp.controls.shape', (320, 50, 320, 56): 'nlp.ns'}, {}), '(nlp.controls.shape, nlp.ns)', True, 'import numpy as np\n'), ((321, 13, 321, 57), 'numpy.random.rand', 'np.random.rand', ({(321, 28, 321, 48): 'nlp.parameters.shape', (321, 50, 321, 56): 'nlp.ns'}, {}), '(nlp.parameters.shape, nlp.ns)', True, 'import numpy as np\n'), ((560, 10, 560, 28), 'bioptim.optimization.non_linear_program.NonLinearProgram', 'NonLinearProgram', ({}, {}), '()', False, 'from bioptim.optimization.non_linear_program import NonLinearProgram\n'), ((575, 4, 575, 22), 'numpy.random.seed', 'np.random.seed', ({(575, 19, 575, 21): '(42)'}, {}), '(42)', True, 'import numpy as np\n'), ((578, 4, 578, 41), 'bioptim.dynamics.configure_problem.ConfigureProblem.initialize', 'ConfigureProblem.initialize', ({(578, 32, 578, 35): 'ocp', (578, 37, 578, 40): 'nlp'}, {}), '(ocp, nlp)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((581, 13, 581, 53), 'numpy.random.rand', 'np.random.rand', ({(581, 28, 581, 44): 'nlp.states.shape', (581, 46, 581, 52): 'nlp.ns'}, {}), '(nlp.states.shape, nlp.ns)', True, 'import numpy as np\n'), ((582, 15, 582, 57), 'numpy.random.rand', 'np.random.rand', ({(582, 30, 582, 48): 'nlp.controls.shape', (582, 50, 582, 56): 'nlp.ns'}, {}), '(nlp.controls.shape, nlp.ns)', True, 'import numpy as np\n'), ((583, 13, 583, 57), 'numpy.random.rand', 'np.random.rand', ({(583, 28, 583, 48): 'nlp.parameters.shape', (583, 50, 583, 56): 'nlp.ns'}, {}), '(nlp.parameters.shape, nlp.ns)', True, 'import numpy as np\n'), ((21, 17, 21, 41), 'bioptim.optimization.optimization_vector.OptimizationVector', 'OptimizationVector', ({(21, 36, 21, 40): 'self'}, {}), '(self)', False, 'from bioptim.optimization.optimization_vector import OptimizationVector\n'), ((40, 47, 40, 109), 'bioptim.dynamics.configure_problem.Dynamics', 'Dynamics', (), '', False, 'from bioptim.dynamics.configure_problem import DynamicsFcn, Dynamics\n'), ((100, 30, 100, 103), 'bioptim.dynamics.configure_problem.Dynamics', 'Dynamics', (), '', False, 'from bioptim.dynamics.configure_problem import DynamicsFcn, Dynamics\n'), ((213, 30, 213, 104), 'bioptim.dynamics.configure_problem.Dynamics', 'Dynamics', (), '', False, 'from bioptim.dynamics.configure_problem import DynamicsFcn, Dynamics\n'), ((301, 8, 306, 9), 'bioptim.dynamics.configure_problem.Dynamics', 'Dynamics', (), '', False, 'from bioptim.dynamics.configure_problem import DynamicsFcn, Dynamics\n'), ((540, 8, 540, 59), 'bioptim.dynamics.dynamics_functions.DynamicsFunctions.apply_parameters', 'DynamicsFunctions.apply_parameters', ({(540, 43, 540, 53): 'parameters', (540, 55, 540, 58): 'nlp'}, {}), '(parameters, nlp)', False, 'from bioptim.dynamics.dynamics_functions import DynamicsFunctions\n'), ((541, 12, 541, 58), 'bioptim.dynamics.dynamics_functions.DynamicsFunctions.get', 'DynamicsFunctions.get', ({(541, 34, 541, 49): "nlp.states['q']", (541, 51, 541, 57): 'states'}, {}), "(nlp.states['q'], states)", False, 'from bioptim.dynamics.dynamics_functions import DynamicsFunctions\n'), ((542, 15, 542, 64), 'bioptim.dynamics.dynamics_functions.DynamicsFunctions.get', 'DynamicsFunctions.get', ({(542, 37, 542, 55): "nlp.states['qdot']", (542, 57, 542, 63): 'states'}, {}), "(nlp.states['qdot'], states)", False, 'from bioptim.dynamics.dynamics_functions import DynamicsFunctions\n'), ((543, 14, 543, 66), 'bioptim.dynamics.dynamics_functions.DynamicsFunctions.get', 'DynamicsFunctions.get', ({(543, 36, 543, 55): "nlp.controls['tau']", (543, 57, 543, 65): 'controls'}, {}), "(nlp.controls['tau'], controls)", False, 'from bioptim.dynamics.dynamics_functions import DynamicsFunctions\n'), ((545, 13, 545, 57), 'bioptim.dynamics.dynamics_functions.DynamicsFunctions.compute_qdot', 'DynamicsFunctions.compute_qdot', ({(545, 44, 545, 47): 'nlp', (545, 49, 545, 50): 'q', (545, 52, 545, 56): 'qdot'}, {}), '(nlp, q, qdot)', False, 'from bioptim.dynamics.dynamics_functions import DynamicsFunctions\n'), ((546, 14, 546, 81), 'bioptim.dynamics.dynamics_functions.DynamicsFunctions.forward_dynamics', 'DynamicsFunctions.forward_dynamics', ({(546, 49, 546, 52): 'nlp', (546, 54, 546, 55): 'q', (546, 57, 546, 61): 'qdot', (546, 63, 546, 66): 'tau', (546, 68, 546, 80): 'with_contact'}, {}), '(nlp, q, qdot, tau, with_contact)', False, 'from bioptim.dynamics.dynamics_functions import DynamicsFunctions\n'), ((551, 8, 551, 54), 'bioptim.dynamics.configure_problem.ConfigureProblem.configure_q', 'ConfigureProblem.configure_q', ({(551, 37, 551, 40): 'nlp', (551, 42, 551, 46): '(True)', (551, 48, 551, 53): '(False)'}, {}), '(nlp, True, False)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((552, 8, 552, 57), 'bioptim.dynamics.configure_problem.ConfigureProblem.configure_qdot', 'ConfigureProblem.configure_qdot', ({(552, 40, 552, 43): 'nlp', (552, 45, 552, 49): '(True)', (552, 51, 552, 56): '(False)'}, {}), '(nlp, True, False)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((553, 8, 553, 56), 'bioptim.dynamics.configure_problem.ConfigureProblem.configure_tau', 'ConfigureProblem.configure_tau', ({(553, 39, 553, 42): 'nlp', (553, 44, 553, 49): '(False)', (553, 51, 553, 55): '(True)'}, {}), '(nlp, False, True)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((554, 8, 554, 105), 'bioptim.dynamics.configure_problem.ConfigureProblem.configure_dynamics_function', 'ConfigureProblem.configure_dynamics_function', (), '', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((572, 30, 572, 109), 'bioptim.dynamics.configure_problem.Dynamics', 'Dynamics', (), '', False, 'from bioptim.dynamics.configure_problem import DynamicsFcn, Dynamics\n'), ((588, 8, 590, 9), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(589, 12, 589, 23): 'x_out[:, (0)]', (589, 25, 589, 115): '[0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, \n    0.6507164, -0.2359716]'}, {}), '(x_out[:, (0)], [0.6118529, 0.785176, \n    0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716])', True, 'import numpy as np\n'), ((591, 8, 591, 94), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(591, 39, 591, 56): 'contact_out[:, (0)]', (591, 58, 591, 93): '[-2.444071, 128.8816865, 2.7245124]'}, {}), '(contact_out[:, (0)], [-2.444071, 128.8816865,\n    2.7245124])', True, 'import numpy as np\n'), ((594, 8, 597, 9), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(595, 12, 595, 23): 'x_out[:, (0)]', (596, 12, 596, 112): '[0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791,\n    1.60445173, 35.80238642]'}, {}), '(x_out[:, (0)], [0.61185289, 0.78517596, \n    0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642]\n    )', True, 'import numpy as np\n'), ((45, 30, 45, 95), 'bioptim.interfaces.biorbd_interface.BiorbdInterface.convert_array_to_external_forces', 'BiorbdInterface.convert_array_to_external_forces', ({(45, 79, 45, 94): 'external_forces'}, {}), '(external_forces)', False, 'from bioptim.interfaces.biorbd_interface import BiorbdInterface\n'), ((59, 12, 62, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(60, 16, 60, 27): 'x_out[:, (0)]', (61, 16, 61, 109): '[0.8631034, 0.3251833, 0.1195942, 0.4937956, -7.7700092, -7.5782306, \n    21.7073786, -16.3059315]'}, {}), '(x_out[:, (0)], [0.8631034, 0.3251833, \n    0.1195942, 0.4937956, -7.7700092, -7.5782306, 21.7073786, -16.3059315])', True, 'import numpy as np\n'), ((63, 12, 63, 102), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(63, 43, 63, 60): 'contact_out[:, (0)]', (63, 62, 63, 101): '[-47.8131136, 111.1726516, -24.4449121]'}, {}), '(contact_out[:, (0)], [-47.8131136, \n    111.1726516, -24.4449121])', True, 'import numpy as np\n'), ((65, 12, 67, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(66, 16, 66, 27): 'x_out[:, (0)]', (66, 29, 66, 119): '[0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, \n    0.6507164, -0.2359716]'}, {}), '(x_out[:, (0)], [0.6118529, 0.785176, \n    0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716])', True, 'import numpy as np\n'), ((68, 12, 68, 98), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(68, 43, 68, 60): 'contact_out[:, (0)]', (68, 62, 68, 97): '[-2.444071, 128.8816865, 2.7245124]'}, {}), '(contact_out[:, (0)], [-2.444071, 128.8816865,\n    2.7245124])', True, 'import numpy as np\n'), ((72, 12, 75, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(73, 16, 73, 27): 'x_out[:, (0)]', (74, 16, 74, 113): '[0.86310343, 0.32518332, 0.11959425, 0.4937956, 0.30731739, -9.97912778, \n    1.15263778, 36.02430956]'}, {}), '(x_out[:, (0)], [0.86310343, 0.32518332, \n    0.11959425, 0.4937956, 0.30731739, -9.97912778, 1.15263778, 36.02430956])', True, 'import numpy as np\n'), ((77, 12, 80, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(78, 16, 78, 27): 'x_out[:, (0)]', (79, 16, 79, 116): '[0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791,\n    1.60445173, 35.80238642]'}, {}), '(x_out[:, (0)], [0.61185289, 0.78517596, \n    0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642]\n    )', True, 'import numpy as np\n'), ((106, 30, 106, 95), 'bioptim.interfaces.biorbd_interface.BiorbdInterface.convert_array_to_external_forces', 'BiorbdInterface.convert_array_to_external_forces', ({(106, 79, 106, 94): 'external_forces'}, {}), '(external_forces)', False, 'from bioptim.interfaces.biorbd_interface import BiorbdInterface\n'), ((120, 12, 136, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(121, 16, 121, 27): 'x_out[:, (0)]', (122, 16, 135, 17): '[0.8631034, 0.3251833, 0.1195942, 0.4937956, -7.7700092, -7.5782306, \n    21.7073786, -16.3059315, 0.8074402, 0.4271078, 0.417411, 0.3232029]'}, {}), '(x_out[:, (0)], [0.8631034, 0.3251833, \n    0.1195942, 0.4937956, -7.7700092, -7.5782306, 21.7073786, -16.3059315, \n    0.8074402, 0.4271078, 0.417411, 0.3232029])', True, 'import numpy as np\n'), ((137, 12, 137, 102), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(137, 43, 137, 60): 'contact_out[:, (0)]', (137, 62, 137, 101): '[-47.8131136, 111.1726516, -24.4449121]'}, {}), '(contact_out[:, (0)], [-47.8131136, \n    111.1726516, -24.4449121])', True, 'import numpy as np\n'), ((139, 12, 155, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(140, 16, 140, 27): 'x_out[:, (0)]', (141, 16, 154, 17): '[0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.32149054, -0.19121314, \n    0.65071636, -0.23597164, 0.38867729, 0.54269608, 0.77224477, 0.72900717]'}, {}), '(x_out[:, (0)], [0.61185289, 0.78517596, \n    0.60754485, 0.80839735, -0.32149054, -0.19121314, 0.65071636, -\n    0.23597164, 0.38867729, 0.54269608, 0.77224477, 0.72900717])', True, 'import numpy as np\n'), ((156, 12, 156, 98), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(156, 43, 156, 60): 'contact_out[:, (0)]', (156, 62, 156, 97): '[-2.444071, 128.8816865, 2.7245124]'}, {}), '(contact_out[:, (0)], [-2.444071, 128.8816865,\n    2.7245124])', True, 'import numpy as np\n'), ((160, 12, 176, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(161, 16, 161, 27): 'x_out[:, (0)]', (162, 16, 175, 17): '[0.86310343, 0.32518332, 0.11959425, 0.4937956, 0.30731739, -9.97912778, \n    1.15263778, 36.02430956, 0.80744016, 0.42710779, 0.417411, 0.32320293]'}, {}), '(x_out[:, (0)], [0.86310343, 0.32518332, \n    0.11959425, 0.4937956, 0.30731739, -9.97912778, 1.15263778, 36.02430956,\n    0.80744016, 0.42710779, 0.417411, 0.32320293])', True, 'import numpy as np\n'), ((178, 12, 194, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(179, 16, 179, 27): 'x_out[:, (0)]', (180, 16, 193, 17): '[0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791,\n    1.60445173, 35.80238642, 0.38867729, 0.54269608, 0.77224477, 0.72900717]'}, {}), '(x_out[:, (0)], [0.61185289, 0.78517596, \n    0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, \n    35.80238642, 0.38867729, 0.54269608, 0.77224477, 0.72900717])', True, 'import numpy as np\n'), ((219, 30, 219, 95), 'bioptim.interfaces.biorbd_interface.BiorbdInterface.convert_array_to_external_forces', 'BiorbdInterface.convert_array_to_external_forces', ({(219, 79, 219, 94): 'external_forces'}, {}), '(external_forces)', False, 'from bioptim.interfaces.biorbd_interface import BiorbdInterface\n'), ((233, 12, 237, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((238, 12, 238, 105), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(238, 43, 238, 60): 'contact_out[:, (0)]', (238, 62, 238, 104): '[109.8086936, 3790.3932439, -3571.7858574]'}, {}), '(contact_out[:, (0)], [109.8086936, \n    3790.3932439, -3571.7858574])', True, 'import numpy as np\n'), ((240, 12, 244, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((245, 12, 245, 105), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(245, 43, 245, 60): 'contact_out[:, (0)]', (245, 62, 245, 104): '[-7.88958997, 329.70828173, -263.55516549]'}, {}), '(contact_out[:, (0)], [-7.88958997, \n    329.70828173, -263.55516549])', True, 'import numpy as np\n'), ((249, 12, 262, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((264, 12, 277, 13), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((313, 30, 313, 95), 'bioptim.interfaces.biorbd_interface.BiorbdInterface.convert_array_to_external_forces', 'BiorbdInterface.convert_array_to_external_forces', ({(313, 79, 313, 94): 'external_forces'}, {}), '(external_forces)', False, 'from bioptim.interfaces.biorbd_interface import BiorbdInterface\n'), ((557, 12, 557, 110), 'bioptim.dynamics.configure_problem.ConfigureProblem.configure_contact_function', 'ConfigureProblem.configure_contact_function', ({(557, 56, 557, 59): 'ocp', (557, 61, 557, 64): 'nlp', (557, 66, 557, 109): 'DynamicsFunctions.forces_from_torque_driven'}, {}), '(ocp, nlp, DynamicsFunctions.\n    forces_from_torque_driven)', False, 'from bioptim.dynamics.configure_problem import ConfigureProblem\n'), ((328, 20, 344, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(329, 24, 329, 35): 'x_out[:, (0)]', (330, 24, 343, 25): '[0.6158501, 0.50313626, 0.64241928, 1.07179622, -33.76217857, 36.21815923, \n    46.87928022, -1.80189035, 53.3914525, 48.30056919, 63.69373374, -\n    28.15700995]'}, {}), '(x_out[:, (0)], [0.6158501, 0.50313626, \n    0.64241928, 1.07179622, -33.76217857, 36.21815923, 46.87928022, -\n    1.80189035, 53.3914525, 48.30056919, 63.69373374, -28.15700995])', True, 'import numpy as np\n'), ((346, 20, 363, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((366, 20, 370, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((372, 20, 376, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((381, 20, 397, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(382, 24, 382, 35): 'x_out[:, (0)]', (383, 24, 396, 25): '[0.6158501, 0.50313626, 0.64241928, 0.91952705, -39.04876174, 45.31837288, \n    55.65557816, 50.47052688, 0.36025589, 58.92377491, 29.70094194, -\n    15.13534937]'}, {}), '(x_out[:, (0)], [0.6158501, 0.50313626, \n    0.64241928, 0.91952705, -39.04876174, 45.31837288, 55.65557816, \n    50.47052688, 0.36025589, 58.92377491, 29.70094194, -15.13534937])', True, 'import numpy as np\n'), ((399, 20, 416, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((419, 20, 422, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(420, 24, 420, 35): 'x_out[:, (0)]', (421, 24, 421, 98): '[0.6158501, 0.50313626, 0.64241928, 0.91952705, -39.04876174, 45.31837288]'}, {}), '(x_out[:, (0)], [0.6158501, 0.50313626, \n    0.64241928, 0.91952705, -39.04876174, 45.31837288])', True, 'import numpy as np\n'), ((424, 20, 428, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((434, 20, 450, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(435, 24, 435, 35): 'x_out[:, (0)]', (436, 24, 449, 25): '[0.6158501, 0.50313626, 0.64241928, 1.07179622, -33.76217857, 36.21815923, \n    46.87928022, -1.80189035, 53.3914525, 48.30056919, 63.69373374, -\n    28.15700995]'}, {}), '(x_out[:, (0)], [0.6158501, 0.50313626, \n    0.64241928, 1.07179622, -33.76217857, 36.21815923, 46.87928022, -\n    1.80189035, 53.3914525, 48.30056919, 63.69373374, -28.15700995])', True, 'import numpy as np\n'), ((452, 20, 469, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((472, 20, 476, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((478, 20, 482, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((487, 20, 503, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(488, 24, 488, 35): 'x_out[:, (0)]', (489, 24, 502, 25): '[0.6158501, 0.50313626, 0.64241928, 0.91952705, -39.04876174, 45.31837288, \n    55.65557816, 50.47052688, 0.36025589, 58.92377491, 29.70094194, -\n    15.13534937]'}, {}), '(x_out[:, (0)], [0.6158501, 0.50313626, \n    0.64241928, 0.91952705, -39.04876174, 45.31837288, 55.65557816, \n    50.47052688, 0.36025589, 58.92377491, 29.70094194, -15.13534937])', True, 'import numpy as np\n'), ((505, 20, 522, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n'), ((525, 20, 528, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', ({(526, 24, 526, 35): 'x_out[:, (0)]', (527, 24, 527, 98): '[0.6158501, 0.50313626, 0.64241928, 0.91952705, -39.04876174, 45.31837288]'}, {}), '(x_out[:, (0)], [0.6158501, 0.50313626, \n    0.64241928, 0.91952705, -39.04876174, 45.31837288])', True, 'import numpy as np\n'), ((530, 20, 534, 21), 'numpy.testing.assert_almost_equal', 'np.testing.assert_almost_equal', (), '', True, 'import numpy as np\n')]