diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 01189eea1e68cfaa080b6a7e78877f035bb1cd91..c752ba21e686c97858256bf19d3b3548891d9714 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -1,4 +1,4 @@ -ADD_SUBDIRECTORY(python) +# ADD_SUBDIRECTORY(python) SET(${PROJECT_NAME}_CPP_TESTS ## without KKT @@ -40,6 +40,7 @@ MACRO(ADD_CPP_UNIT_TEST NAME PKGS) SET(MODULE_NAME "${NAME}Test") STRING(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME}) ADD_TEST_CFLAGS(${unittest_name} "-DTEST_MODULE_NAME=${MODULE_NAME}") + ADD_TEST_CFLAGS(${unittest_name} "-DBOOST_TEST_MODULE=${MODULE_NAME}") FOREACH(PKG ${PKGS}) PKG_CONFIG_USE_DEPENDENCY(${unittest_name} ${PKG}) @@ -66,5 +67,5 @@ ENDFOREACH(TEST ${${PROJECT_NAME}_CPP_TESTS}) IF(BUILD_PYTHON_INTERFACE) - ADD_SUBDIRECTORY(bindings) + # ADD_SUBDIRECTORY(bindings) ENDIF(BUILD_PYTHON_INTERFACE) \ No newline at end of file diff --git a/unittest/test_state.cpp b/unittest/test_state.cpp index 123b0565e294cb81e73ef9306f3685d62efe015d..bcd9608d09b14bb8d2dcc3edb1ed1a17687d161c 100644 --- a/unittest/test_state.cpp +++ b/unittest/test_state.cpp @@ -26,29 +26,48 @@ using namespace boost::unit_test; +template<class PtrType> +void delete_pointer(PtrType* ptr) +{ + if(ptr != NULL) + { + delete ptr; + ptr = NULL; + } +} + class StateAbstractFactory { public: - crocoddyl::StateAbstract& get_state() { return *state_; } - boost::shared_ptr<crocoddyl::StateAbstract> state_; + crocoddyl::StateAbstract* get_state() { return state_; } + crocoddyl::StateAbstract* state_; }; class StateVectorFactory : public StateAbstractFactory { public: StateVectorFactory(int nx) : StateAbstractFactory() { - state_vector_ = boost::make_shared<crocoddyl::StateVector>(nx); + std::cout << "creating the StateVector" << std::endl; + state_vector_ = new crocoddyl::StateVector(nx); state_ = state_vector_; + std::cout << "created the StateVector" << std::endl; } - boost::shared_ptr<crocoddyl::StateVector> state_vector_; + ~StateVectorFactory() + { + std::cout << "deleting the StateVector" << std::endl; + delete_pointer(state_vector_); + std::cout << "deleted the StateVector" << std::endl; + } + crocoddyl::StateVector* state_vector_; }; class StateMultibodyFactory : public StateAbstractFactory { public: StateMultibodyFactory(const std::string& urdf_file = "", bool free_flyer = true) : StateAbstractFactory() { - pinocchio_model_.reset(new pinocchio::Model()); + std::cout << "creating the state" << std::endl; + pinocchio_model_ = new pinocchio::Model(); if (urdf_file.size() != 0) { if (free_flyer) { - free_flyer_.reset(new pinocchio::JointModelFreeFlyer()); - pinocchio::urdf::buildModel(urdf_file, *free_flyer_, *pinocchio_model_, true); + free_flyer_joint_ = new pinocchio::JointModelFreeFlyer(); + pinocchio::urdf::buildModel(urdf_file, *free_flyer_joint_, *pinocchio_model_); pinocchio_model_->lowerPositionLimit.head<3>().fill(-1.0); pinocchio_model_->upperPositionLimit.head<3>().fill(1.0); } else { @@ -57,15 +76,18 @@ class StateMultibodyFactory : public StateAbstractFactory { } else { pinocchio::buildModels::humanoidRandom(*pinocchio_model_, free_flyer); } - - state_multibody_.reset(new crocoddyl::StateMultibody(*pinocchio_model_)); + state_multibody_ = new crocoddyl::StateMultibody(*pinocchio_model_); state_ = state_multibody_; std::cout << "created the state" << std::endl; } ~StateMultibodyFactory() { + delete_pointer(free_flyer_joint_); + delete_pointer(state_multibody_); + delete_pointer(pinocchio_model_); + std::cout << "deleting the StateMultibodyFactory "; - if (free_flyer_) + if (free_flyer_joint_) { std::cout << "with free flyer."; } @@ -75,127 +97,129 @@ class StateMultibodyFactory : public StateAbstractFactory { } std::cout << std::endl; } - boost::shared_ptr<pinocchio::JointModelFreeFlyer> free_flyer_; - boost::shared_ptr<crocoddyl::StateMultibody> state_multibody_; - boost::shared_ptr<pinocchio::Model> pinocchio_model_; + pinocchio::JointModelFreeFlyer* free_flyer_joint_; + crocoddyl::StateMultibody* state_multibody_; + pinocchio::Model* pinocchio_model_; }; -void test_state_dimension(StateAbstractFactory& factory, int nx) { +//----------------------------------------------------------------------------// + +void test_state_dimension(StateAbstractFactory* factory, int nx) { std::cout << "test_state_dimension" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Checking the dimension of zero and random states - BOOST_CHECK(state.zero().size() == nx); - BOOST_CHECK(state.rand().size() == nx); + BOOST_CHECK(state->zero().size() == nx); + BOOST_CHECK(state->rand().size() == nx); std::cout << "test_state_dimension end" << std::endl; } -void test_integrate_against_difference(StateAbstractFactory& factory) { +void test_integrate_against_difference(StateAbstractFactory* factory) { std::cout << "test_integrate_against_difference" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random states - Eigen::VectorXd x1 = state.rand(); - Eigen::VectorXd x2 = state.rand(); + Eigen::VectorXd x1 = state->rand(); + Eigen::VectorXd x2 = state->rand(); // Computing x2 by integrating its difference - Eigen::VectorXd dx(state.get_ndx()); - state.diff(x1, x2, dx); - Eigen::VectorXd x2i(state.get_nx()); - state.integrate(x1, dx, x2i); + Eigen::VectorXd dx(state->get_ndx()); + state->diff(x1, x2, dx); + Eigen::VectorXd x2i(state->get_nx()); + state->integrate(x1, dx, x2i); - Eigen::VectorXd dxi(state.get_ndx()); - state.diff(x2i, x2, dxi); + Eigen::VectorXd dxi(state->get_ndx()); + state->diff(x2i, x2, dxi); // Checking that both states agree BOOST_CHECK(dxi.isMuchSmallerThan(1.0, 1e-9)); std::cout << "test_integrate_against_difference end" << std::endl; } -void test_difference_against_integrate(StateAbstractFactory& factory) { +void test_difference_against_integrate(StateAbstractFactory* factory) { std::cout << "test_difference_against_integrate" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random states - Eigen::VectorXd x = state.rand(); - Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx()); + Eigen::VectorXd x = state->rand(); + Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx()); // Computing dx by differentiation of its integrate - Eigen::VectorXd xidx(state.get_nx()); - state.integrate(x, dx, xidx); - Eigen::VectorXd dxd(state.get_ndx()); - state.diff(x, xidx, dxd); + Eigen::VectorXd xidx(state->get_nx()); + state->integrate(x, dx, xidx); + Eigen::VectorXd dxd(state->get_ndx()); + state->diff(x, xidx, dxd); // Checking that both states agree BOOST_CHECK((dxd - dx).isMuchSmallerThan(1.0, 1e-9)); std::cout << "test_difference_against_integrate end" << std::endl; } -void test_Jdiff_firstsecond(StateAbstractFactory& factory) { +void test_Jdiff_firstsecond(StateAbstractFactory* factory) { std::cout << "test_Jdiff_firstsecond" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random values for the initial and terminal states - Eigen::VectorXd x1 = state.rand(); - Eigen::VectorXd x2 = state.rand(); + Eigen::VectorXd x1 = state->rand(); + Eigen::VectorXd x2 = state->rand(); // Computing the partial derivatives of the difference function separately - Eigen::MatrixXd Jdiff_tmp(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_second(state.get_ndx(), state.get_ndx()); - state.Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first); - state.Jdiff(x1, x2, Jdiff_tmp, Jdiff_second, crocoddyl::second); + Eigen::MatrixXd Jdiff_tmp(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_second(state->get_ndx(), state->get_ndx()); + state->Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first); + state->Jdiff(x1, x2, Jdiff_tmp, Jdiff_second, crocoddyl::second); // Computing the partial derivatives of the difference function separately - Eigen::MatrixXd Jdiff_both_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_both_second(state.get_ndx(), state.get_ndx()); - state.Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second); + Eigen::MatrixXd Jdiff_both_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_both_second(state->get_ndx(), state->get_ndx()); + state->Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second); BOOST_CHECK((Jdiff_first - Jdiff_both_first).isMuchSmallerThan(1.0, 1e-9)); BOOST_CHECK((Jdiff_second - Jdiff_both_second).isMuchSmallerThan(1.0, 1e-9)); std::cout << "test_Jdiff_firstsecond end" << std::endl; } -void test_Jint_firstsecond(StateAbstractFactory& factory) { +void test_Jint_firstsecond(StateAbstractFactory* factory) { std::cout << "test_Jint_firstsecond" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random values for the initial and terminal states - Eigen::VectorXd x = state.rand(); - Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx()); + Eigen::VectorXd x = state->rand(); + Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx()); // Computing the partial derivatives of the difference function separately - Eigen::MatrixXd Jint_tmp(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_second(state.get_ndx(), state.get_ndx()); - state.Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first); - state.Jintegrate(x, dx, Jint_tmp, Jint_second, crocoddyl::second); + Eigen::MatrixXd Jint_tmp(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_second(state->get_ndx(), state->get_ndx()); + state->Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first); + state->Jintegrate(x, dx, Jint_tmp, Jint_second, crocoddyl::second); // Computing the partial derivatives of the interence function separately - Eigen::MatrixXd Jint_both_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_both_second(state.get_ndx(), state.get_ndx()); - state.Jintegrate(x, dx, Jint_both_first, Jint_both_second); + Eigen::MatrixXd Jint_both_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_both_second(state->get_ndx(), state->get_ndx()); + state->Jintegrate(x, dx, Jint_both_first, Jint_both_second); BOOST_CHECK((Jint_first - Jint_both_first).isMuchSmallerThan(1.0, 1e-9)); BOOST_CHECK((Jint_second - Jint_both_second).isMuchSmallerThan(1.0, 1e-9)); std::cout << "test_Jint_firstsecond end" << std::endl; } -void test_Jdiff_num_diff_firstsecond(StateAbstractFactory& factory) { +void test_Jdiff_num_diff_firstsecond(StateAbstractFactory* factory) { std::cout << "test_Jdiff_num_diff_firstsecond" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random values for the initial and terminal states - Eigen::VectorXd x1 = state.rand(); - Eigen::VectorXd x2 = state.rand(); + Eigen::VectorXd x1 = state->rand(); + Eigen::VectorXd x2 = state->rand(); // Get the num diff state - crocoddyl::StateNumDiff state_num_diff(state); + crocoddyl::StateNumDiff state_num_diff(*state); // Computing the partial derivatives of the difference function separately - Eigen::MatrixXd Jdiff_num_diff_tmp(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_num_diff_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_num_diff_second(state.get_ndx(), state.get_ndx()); + Eigen::MatrixXd Jdiff_num_diff_tmp(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_num_diff_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_num_diff_second(state->get_ndx(), state->get_ndx()); state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_first, Jdiff_num_diff_tmp, crocoddyl::first); state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_tmp, Jdiff_num_diff_second, crocoddyl::second); // Computing the partial derivatives of the difference function separately - Eigen::MatrixXd Jdiff_num_diff_both_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_num_diff_both_second(state.get_ndx(), state.get_ndx()); + Eigen::MatrixXd Jdiff_num_diff_both_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_num_diff_both_second(state->get_ndx(), state->get_ndx()); state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_both_first, Jdiff_num_diff_both_second); BOOST_CHECK((Jdiff_num_diff_first - Jdiff_num_diff_both_first).isMuchSmallerThan(1.0, 1e-9)); @@ -203,26 +227,26 @@ void test_Jdiff_num_diff_firstsecond(StateAbstractFactory& factory) { std::cout << "test_Jdiff_num_diff_firstsecond end" << std::endl; } -void test_Jint_num_diff_firstsecond(StateAbstractFactory& factory) { +void test_Jint_num_diff_firstsecond(StateAbstractFactory* factory) { std::cout << "test_Jint_num_diff_firstsecond" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random values for the initial and terminal states - Eigen::VectorXd x = state.rand(); - Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx()); + Eigen::VectorXd x = state->rand(); + Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx()); // Get the num diff state - crocoddyl::StateNumDiff state_num_diff(state); + crocoddyl::StateNumDiff state_num_diff(*state); // Computing the partial derivatives of the difference function separately - Eigen::MatrixXd Jint_num_diff_tmp(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_num_diff_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_num_diff_second(state.get_ndx(), state.get_ndx()); + Eigen::MatrixXd Jint_num_diff_tmp(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_num_diff_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_num_diff_second(state->get_ndx(), state->get_ndx()); state_num_diff.Jintegrate(x, dx, Jint_num_diff_first, Jint_num_diff_tmp, crocoddyl::first); state_num_diff.Jintegrate(x, dx, Jint_num_diff_tmp, Jint_num_diff_second, crocoddyl::second); // Computing the partial derivatives of the interence function separately - Eigen::MatrixXd Jint_num_diff_both_first(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_num_diff_both_second(state.get_ndx(), state.get_ndx()); + Eigen::MatrixXd Jint_num_diff_both_first(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_num_diff_both_second(state->get_ndx(), state->get_ndx()); state_num_diff.Jintegrate(x, dx, Jint_num_diff_both_first, Jint_num_diff_both_second); BOOST_CHECK((Jint_num_diff_first - Jint_num_diff_both_first).isMuchSmallerThan(1.0, 1e-9)); @@ -230,23 +254,23 @@ void test_Jint_num_diff_firstsecond(StateAbstractFactory& factory) { std::cout << "test_Jint_num_diff_firstsecond end" << std::endl; } -void test_Jdiff_against_numdiff(StateAbstractFactory& factory, double num_diff_modifier) { +void test_Jdiff_against_numdiff(StateAbstractFactory* factory, double num_diff_modifier) { std::cout << "test_Jdiff_against_numdiff" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random values for the initial and terminal states - Eigen::VectorXd x1 = state.rand(); - Eigen::VectorXd x2 = state.rand(); + Eigen::VectorXd x1 = state->rand(); + Eigen::VectorXd x2 = state->rand(); // Computing the partial derivatives of the difference function analytically - Eigen::MatrixXd Jdiff_1(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_2(state.get_ndx(), state.get_ndx()); - state.Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first); - state.Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second); + Eigen::MatrixXd Jdiff_1(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_2(state->get_ndx(), state->get_ndx()); + state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first); + state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second); // Computing the partial derivatives of the difference function numerically - crocoddyl::StateNumDiff state_num_diff(state); - Eigen::MatrixXd Jdiff_num_1(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdiff_num_2(state.get_ndx(), state.get_ndx()); + crocoddyl::StateNumDiff state_num_diff(*state); + Eigen::MatrixXd Jdiff_num_1(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdiff_num_2(state->get_ndx(), state->get_ndx()); state_num_diff.Jdiff(x1, x2, Jdiff_num_1, Jdiff_num_2); // Checking the partial derivatives against NumDiff @@ -257,22 +281,22 @@ void test_Jdiff_against_numdiff(StateAbstractFactory& factory, double num_diff_m std::cout << "test_Jdiff_against_numdiff end" << std::endl; } -void test_Jintegrate_against_numdiff(StateAbstractFactory& factory, double num_diff_modifier) { +void test_Jintegrate_against_numdiff(StateAbstractFactory* factory, double num_diff_modifier) { std::cout << "test_Jintegrate_against_numdiff" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random values for the initial state and its rate of change - Eigen::VectorXd x = state.rand(); - Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx()); + Eigen::VectorXd x = state->rand(); + Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx()); // Computing the partial derivatives of the difference function analytically - Eigen::MatrixXd Jint_1(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_2(state.get_ndx(), state.get_ndx()); - state.Jintegrate(x, dx, Jint_1, Jint_2); + Eigen::MatrixXd Jint_1(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_2(state->get_ndx(), state->get_ndx()); + state->Jintegrate(x, dx, Jint_1, Jint_2); // Computing the partial derivatives of the difference function numerically - crocoddyl::StateNumDiff state_num_diff(state); - Eigen::MatrixXd Jint_num_1(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jint_num_2(state.get_ndx(), state.get_ndx()); + crocoddyl::StateNumDiff state_num_diff(*state); + Eigen::MatrixXd Jint_num_1(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jint_num_2(state->get_ndx(), state->get_ndx()); state_num_diff.Jintegrate(x, dx, Jint_num_1, Jint_num_2); // Checking the partial derivatives against NumDiff @@ -283,22 +307,22 @@ void test_Jintegrate_against_numdiff(StateAbstractFactory& factory, double num_d std::cout << "test_Jintegrate_against_numdiff end" << std::endl; } -void test_Jdiff_and_Jintegrate_are_inverses(StateAbstractFactory& factory) { +void test_Jdiff_and_Jintegrate_are_inverses(StateAbstractFactory* factory) { std::cout << "test_Jdiff_and_Jintegrate_are_inverses" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random states - Eigen::VectorXd x1 = state.rand(); - Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx()); - Eigen::VectorXd x2(state.get_nx()); - state.integrate(x1, dx, x2); + Eigen::VectorXd x1 = state->rand(); + Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx()); + Eigen::VectorXd x2(state->get_nx()); + state->integrate(x1, dx, x2); // Computing the partial derivatives of the integrate and difference function - Eigen::MatrixXd Jx(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdx(state.get_ndx(), state.get_ndx()); - state.Jintegrate(x1, dx, Jx, Jdx); - Eigen::MatrixXd J1(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd J2(state.get_ndx(), state.get_ndx()); - state.Jdiff(x1, x2, J1, J2); + Eigen::MatrixXd Jx(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdx(state->get_ndx(), state->get_ndx()); + state->Jintegrate(x1, dx, Jx, Jdx); + Eigen::MatrixXd J1(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd J2(state->get_ndx(), state->get_ndx()); + state->Jdiff(x1, x2, J1, J2); // Checking that Jdiff and Jintegrate are inverses Eigen::MatrixXd dX_dDX = Jdx; @@ -307,44 +331,44 @@ void test_Jdiff_and_Jintegrate_are_inverses(StateAbstractFactory& factory) { std::cout << "test_Jdiff_and_Jintegrate_are_inverses end" << std::endl; } -void test_velocity_from_Jintegrate_Jdiff(StateAbstractFactory& factory) { +void test_velocity_from_Jintegrate_Jdiff(StateAbstractFactory* factory) { std::cout << "test_velocity_from_Jintegrate_Jdiff" << std::endl; - crocoddyl::StateAbstract& state = factory.get_state(); + crocoddyl::StateAbstract* state = factory->get_state(); // Generating random states - Eigen::VectorXd x1 = state.rand(); - Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx()); - Eigen::VectorXd x2(state.get_nx()); - state.integrate(x1, dx, x2); - Eigen::VectorXd eps = Eigen::VectorXd::Random(state.get_ndx()); + Eigen::VectorXd x1 = state->rand(); + Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx()); + Eigen::VectorXd x2(state->get_nx()); + state->integrate(x1, dx, x2); + Eigen::VectorXd eps = Eigen::VectorXd::Random(state->get_ndx()); double h = 1e-8; // Computing the partial derivatives of the integrate and difference function - Eigen::MatrixXd Jx(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd Jdx(state.get_ndx(), state.get_ndx()); - state.Jintegrate(x1, dx, Jx, Jdx); - Eigen::MatrixXd J1(state.get_ndx(), state.get_ndx()); - Eigen::MatrixXd J2(state.get_ndx(), state.get_ndx()); - state.Jdiff(x1, x2, J1, J2); + Eigen::MatrixXd Jx(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd Jdx(state->get_ndx(), state->get_ndx()); + state->Jintegrate(x1, dx, Jx, Jdx); + Eigen::MatrixXd J1(state->get_ndx(), state->get_ndx()); + Eigen::MatrixXd J2(state->get_ndx(), state->get_ndx()); + state->Jdiff(x1, x2, J1, J2); // Checking that computed velocity from Jintegrate Eigen::MatrixXd dX_dDX = Jdx; - Eigen::VectorXd x2eps(state.get_nx()); - state.integrate(x1, dx + eps * h, x2eps); - Eigen::VectorXd x2_eps(state.get_ndx()); - state.diff(x2, x2eps, x2_eps); + Eigen::VectorXd x2eps(state->get_nx()); + state->integrate(x1, dx + eps * h, x2eps); + Eigen::VectorXd x2_eps(state->get_ndx()); + state->diff(x2, x2eps, x2_eps); BOOST_CHECK((dX_dDX * eps - x2_eps / h).isMuchSmallerThan(1.0, 1e-3)); // Checking the velocity computed from Jdiff - Eigen::VectorXd x = state.rand(); + Eigen::VectorXd x = state->rand(); dx.setZero(); - state.diff(x1, x, dx); - Eigen::VectorXd x2i(state.get_nx()); - state.integrate(x, eps * h, x2i); - Eigen::VectorXd dxi(state.get_ndx()); - state.diff(x1, x2i, dxi); + state->diff(x1, x, dx); + Eigen::VectorXd x2i(state->get_nx()); + state->integrate(x, eps * h, x2i); + Eigen::VectorXd dxi(state->get_ndx()); + state->diff(x1, x2i, dxi); J1.setZero(); J2.setZero(); - state.Jdiff(x1, x, J1, J2); + state->Jdiff(x1, x, J1, J2); BOOST_CHECK((J2 * eps - (-dx + dxi) / h).isMuchSmallerThan(1.0, 1e-3)); std::cout << "test_velocity_from_Jintegrate_Jdiff end" << std::endl; } @@ -353,74 +377,74 @@ void register_state_vector_unit_tests() { int nx = 10; double num_diff_modifier = 1e4; - framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_state_dimension, StateVectorFactory(nx), nx))); + framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_state_dimension, new StateVectorFactory(nx), nx))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, StateVectorFactory(nx)))); + BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, new StateVectorFactory(nx)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, StateVectorFactory(nx)))); + BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, new StateVectorFactory(nx)))); - framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, StateVectorFactory(nx)))); + framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, new StateVectorFactory(nx)))); - framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, StateVectorFactory(nx)))); + framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, new StateVectorFactory(nx)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jdiff_num_diff_firstsecond, StateVectorFactory(nx)))); + BOOST_TEST_CASE(boost::bind(&test_Jdiff_num_diff_firstsecond, new StateVectorFactory(nx)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, StateVectorFactory(nx)))); + BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, new StateVectorFactory(nx)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, StateVectorFactory(nx), num_diff_modifier))); + BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, new StateVectorFactory(nx), num_diff_modifier))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jintegrate_against_numdiff, StateVectorFactory(nx), num_diff_modifier))); + BOOST_TEST_CASE(boost::bind(&test_Jintegrate_against_numdiff, new StateVectorFactory(nx), num_diff_modifier))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, StateVectorFactory(nx)))); + BOOST_TEST_CASE(boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, new StateVectorFactory(nx)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_velocity_from_Jintegrate_Jdiff, StateVectorFactory(nx)))); + BOOST_TEST_CASE(boost::bind(&test_velocity_from_Jintegrate_Jdiff, new StateVectorFactory(nx)))); } void register_state_multibody_unit_tests(const std::string& urdf_file = "", bool free_flyer = true) { double num_diff_modifier = 1e4; - StateMultibodyFactory factory = StateMultibodyFactory(urdf_file, free_flyer); + StateMultibodyFactory* factory = new StateMultibodyFactory(urdf_file, free_flyer); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_state_dimension, StateMultibodyFactory(urdf_file, free_flyer), - factory.pinocchio_model_->nq + factory.pinocchio_model_->nv))); + BOOST_TEST_CASE(boost::bind(&test_state_dimension, new StateMultibodyFactory(urdf_file, free_flyer), + factory->pinocchio_model_->nq + factory->pinocchio_model_->nv))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, StateMultibodyFactory(urdf_file, free_flyer)))); + BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, new StateMultibodyFactory(urdf_file, free_flyer)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, StateMultibodyFactory(urdf_file, free_flyer)))); + BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, new StateMultibodyFactory(urdf_file, free_flyer)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, StateMultibodyFactory(urdf_file, free_flyer)))); + BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, StateMultibodyFactory(urdf_file, free_flyer)))); + BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jdiff_num_diff_firstsecond, StateMultibodyFactory(urdf_file, free_flyer)))); + BOOST_TEST_CASE(boost::bind(&test_Jdiff_num_diff_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer)))); framework::master_test_suite().add( - BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, StateMultibodyFactory(urdf_file, free_flyer)))); + BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer)))); framework::master_test_suite().add(BOOST_TEST_CASE( - boost::bind(&test_Jdiff_against_numdiff, StateMultibodyFactory(urdf_file, free_flyer), num_diff_modifier))); + boost::bind(&test_Jdiff_against_numdiff, new StateMultibodyFactory(urdf_file, free_flyer), num_diff_modifier))); framework::master_test_suite().add(BOOST_TEST_CASE( - boost::bind(&test_Jintegrate_against_numdiff, StateMultibodyFactory(urdf_file, free_flyer), num_diff_modifier))); + boost::bind(&test_Jintegrate_against_numdiff, new StateMultibodyFactory(urdf_file, free_flyer), num_diff_modifier))); framework::master_test_suite().add(BOOST_TEST_CASE( - boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, StateMultibodyFactory(urdf_file, free_flyer)))); + boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, new StateMultibodyFactory(urdf_file, free_flyer)))); framework::master_test_suite().add(BOOST_TEST_CASE( - boost::bind(&test_velocity_from_Jintegrate_Jdiff, StateMultibodyFactory(urdf_file, free_flyer)))); + boost::bind(&test_velocity_from_Jintegrate_Jdiff, new StateMultibodyFactory(urdf_file, free_flyer)))); } //____________________________________________________________________________//